1

     2

#include "PhysicsObject.hh"

     3

#include "Engine.hh"

     4

     5

#include <cmath>

     6

     7

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity, bool enabled) :

     8

    world(world), position(position), velocity(velocity), mass(mass), inAir(true), aim(0), facingRight(true), alive(false), shouldDelete(false)

     9

{  

    10

    if (enabled)

    11

        enable();  

    12

}

    13

    14

PhysicsObject::~PhysicsObject (void) {

    15

//    Engine::log(DEBUG, "PhysicsObject.destructor") << this /* << ": objects.size=" << ((int) world.objects.size()) */;

    16

}

    17

    18

/**

    19

 * Player walks on floor.

    20

 */

    21

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

    22

    // which way we are walking

    23

    float deltaX = right ? partial : -partial;

    24

    Vector reached = this->position;

    25

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

    26

        return reached+Vector(deltaX, 0);

    27

    }

    28

    // Is there upward ramp

    29

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

    30

        // Yes. Then we check n pixels up

    31

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

    32

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

    33

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

    34

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

    35

                break;

    36

            }

    37

        }

    38

    } else {

    39

        // Or downward ramp or flat

    40

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

    41

    42

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

    43

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

    44

            // but will fall one pixel down

    45

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

    46

                reached = position+Vector(deltaX, i);

    47

            } else {

    48

                break;

    49

            }

    50

    51

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

    52

            if (i >= 2) {

    53

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

    54

                this->inAir = true;

    55

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

    56

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

    57

                reached.y -= 2;

    58

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

    59

                break;

    60

            }

    61

        }

    62

    }

    63

    // And we return where we got

    64

    return reached;

    65

}

    66

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

    67

    float velocity = PLAYER_WALK_SPEED;

    68

    float walkAmount = (velocity*dt)/1000;

    69

    Vector reached = this->position;

    70

    while (walkAmount > 0 && !this->inAir) {

    71

        this->position = walk_one_step((1 < walkAmount ? 1 : walkAmount), right);

    72

        walkAmount--;

    73

    }

    74

    // TODO: Should the remaining walkAmount be handled somehow?

    75

}

    76

    77

/**

    78

 * Makes the player jump in the air.

    79

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

    80

 */

    81

void PhysicsObject::jump (int direction) {

    82

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

    83

    if (!this->inAir) {

    84

 	    velocity.y = -100;

    85

        switch (direction) {

    86

            case 1:

    87

                velocity.x += 20;

    88

                break;

    89

            case -1:

    90

                velocity.x -= 20;

    91

                break;

    92

            case 0:

    93

                break;

    94

            default:

    95

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

    96

        }

    97

	    inAir = true;

    98

    }

    99

}

   100

   101

bool PhysicsObject::possibleLocation (Vector loc) {

   102

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

   103

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

   104

            return false;

   105

    }

   106

    return true;

   107

}

   108

   109

/**

   110

 * Updates object speed and position. This function organises force

   111

 * integration and collision detection.

   112

 */   

   113

void PhysicsObject::updatePosition (TimeMS dt) {

   114

    // Add gravity to the force queue

   115

    forceq.push(world.gravity);

   116

   129

    // Go trough every force in the queue

   130

    Force total;

   131

    while (!forceq.empty()) {

   132

        total += forceq.front();

   133

        forceq.pop();

   134

    }

   135

   136

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

   137

    // set inAir false

   138

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

   139

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

   140

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

   141

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

   142

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

   143

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

   144

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

   145

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

   146

            this->inAir = true;

   147

    }

   148

   149

    if(!possibleLocation(position)) {

   150

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

   151

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

   152

        //return;

   153

    }

   154

   155

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

   156

    // otherwise integrate the new position and velocity

   157

    if (!this->inAir) {

   158

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

   159

        if (total.x != 0) {

   160

            walk(dt, total.x > 0);

   161

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

   162

        }

   163

    }

   164

   165

    if (!possibleLocation(position))

   166

        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position;

   167

   168

    Vector newPosition;

   169

    Vector velAfterTick;

   170

    // Calculate new position and velocity to the given references

   171

    integrate(total, dt, newPosition, velAfterTick);

   172

    this->velocity = velAfterTick;

   173

   174

   175

    // Collision detection

   176

    bool collided = false;

   177

   178

    const Vector diffVec = newPosition-position;

   179

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

   180

    Vector reached = position;

   181

   182

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

   183

        reached += unitVector;

   184

        // Check if any of the shapes points collide

   185

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

   186

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

   187

                if (inAir) {

   188

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

   189

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

   190

                                                 reached-unitVector+shape[i]));

   191

                    //this->velocity *= COLLISION_ELASTICITY;

   192

                }

   193

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

   194

                collided = true;

   195

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

   196

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

   197

                }

   198

                break;

   199

            }

   200

        }

   201

        if (collided)

   202

            break;

   203

//        reached += unitVector;

   204

    }

   205

   206

   207

    if (!possibleLocation(reached))

   208

        Engine::log(DEBUG, "PhysicsObject.updatePosition") << "impossible location: " << position << ", diffVec=" << diffVec;

   209

   210

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

   211

    if (!collided) {

   212

        if (!possibleLocation(newPosition)) {

   213

            newPosition = reached;

   214

        } else {

   215

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

   216

        }

   217

   218

        this->position = newPosition;

   219

   220

    } else {

   221

        newPosition = reached;

   222

        this->position = newPosition;

   223

   224

        // the following may delete this object, so it must be the last thing called

   225

        onCollision();

   226

   227

        return;

   228

    }

   229

}

   230

   231

/**

   232

 * Bounces from straight wall in any direction.

   233

 * Direction given as normal of that wall

   234

 */

   235

void PhysicsObject::bounce (Vector normal) {

   236

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

   237

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

   238

        Vector nvel = velocity;

   239

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

   240

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

   241

        velocity = nvel;

   242

        // We lose some of our speed on collision

   243

        this->velocity *= this->collision_elasticity;

   244

    }

   245

}

   246

   247

/**

   248

 * Integrates given force over time and stores new position to

   249

 * posAfterTick and new velocity to velAfterTick.

   250

 * @param force Force vector.

   251

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

   252

 */

   253

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

   254

    posAfterTick = position;

   255

    velAfterTick = velocity;

   256

    Derivative tmpd;

   257

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

   258

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

   259

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

   260

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

   261

   262

   263

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

   264

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

   265

   266

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

   267

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

   268

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

   269

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

   270

}

   271

   272

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

   273

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

   274

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

   275

   276

    Derivative out;

   277

    out.dx = curVel;

   278

    out.dv = acceleration(force);

   279

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

   280

    return out;

   281

}

   282

   283

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

   284

    return (force/mass);

   285

}

   286

   287

void PhysicsObject::applyForce (Force force) {

   288

    // Add applied force to the queue

   289

    forceq.push(force);

   290

}

   291

   292

void PhysicsObject::changeAim(float da) {

   293

    this->aim += da;

   294

   295

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

   296

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

   297

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

   298

}

   299

   300

void PhysicsObject::setFacing(bool facingRight) {

   301

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

   302

    this->facingRight = facingRight;

   303

}

   304

   305

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

   306

    this->position = position;

   307

    this->velocity = velocity;

   308

    this->inAir = inAir;

   309

    this->facingRight = facingRight;

   310

    this->aim = aim;

   311

}

   312

   313

Vector PhysicsObject::getPosition () {

   314

    return this->position;

   315

}

   316

   317

Vector PhysicsObject::getVelocity () {

   318

    return this->velocity;

   319

}

   320

   321

bool PhysicsObject::getFacing() {

   322

    return this->facingRight;

   323

}

   324

   325

float PhysicsObject::getAim() {

   326

    return this->aim;

   327

}

   328

   329

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

   330

    return this->shape;

   331

}

   332

   333

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

   334

    this->shape = shape;

   335

}

   336

   341

void PhysicsObject::tick (TimeMS tick_length) {

   342

    this->updatePosition(tick_length);

   343

}

   344

   345

void PhysicsObject::enable (void) {

   346

    alive = true;

   347

    world.addPhysicsObject(this);

   348

}

   349

   350

void PhysicsObject::disable (void) {

   351

    alive = false;

   352

}

   353

   354

void PhysicsObject::destroy (void) {

   355

    alive = false;

   356

    shouldDelete = true;

   357

}

   358

   359

bool PhysicsObject::isDestroyed (void) {

   360

    return !alive;

   361

}

   362

   363

bool PhysicsObject::removeIfDestroyed (void) {

   364

    if (!alive) {

   365

        if (shouldDelete)

   366

            delete this;

   367

   368

        return true;

   369

   370

    } else {

   371

        return false;

   372

    }

   373

}

   374

   377

void PhysicsObject::draw(CL_GraphicContext *gc) {

   378

    CL_Quad player(

   379

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

   380

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

   381

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

   382

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

   383

                   );

   384

   385

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

   386

   387

    const uint16_t chlen = 10;

   388

    uint16_t x = player.center().x;

   389

    uint16_t y = player.center().y;

   390

    if (facingRight) {

   391

        gc->draw_line(x, y,

   392

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

   393

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

   394

                      CL_Color::black);

   395

    } else {

   396

        gc->draw_line(x, y,

   397

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

   398

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

   399

                      CL_Color::black);

   400

    }

   401

}

   402