1

#include "PhysicsObject.hh"

     2

#include "Engine.hh"

     3

     4

#include <cmath>

     5

#include <utility>

     6

#include <queue>

     7

     8

PhysicsObject::PhysicsObject (PhysicsWorld &world, float mass, Vector position, Vector velocity, ObjectType type, 

     9

            float collision_elasticity, bool enabled) :

    10

    world(world), 

    11

    position(position), 

    12

    previousPosition(position),

    13

    velocity(velocity), 

    14

    mass(mass), 

    15

    inAir(true), 

    16

    collision_elasticity(collision_elasticity),

    17

    aim(0), 

    18

    facing(FACING_RIGHT), 

    19

    alive(false),

    20

    shouldDelete(false), 

    21

    type(type), 

    22

    pivot(NULL)

    23

{  

    24

    if (enabled)

    25

        enable();  

    26

}

    27

    28

PhysicsObject::~PhysicsObject (void) {

    29

    30

}

    31

    32

/*

    33

 * Player walks on floor.

    34

 */

    35

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

    36

    // which way we are walking

    37

    float deltaX = right ? partial : -partial;

    38

    39

    Vector reached = this->position;

    40

    41

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

    42

        return reached + Vector(deltaX, 0);

    43

    }

    44

    45

    // Is there upward ramp

    46

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

    47

        // Yes. Then we check n pixels up

    48

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

    49

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

    50

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

    51

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

    52

                break;

    53

            }

    54

        }

    55

    } else {

    56

        // Or downward ramp or flat

    57

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

    58

    59

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

    60

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

    61

            // but will fall one pixel down

    62

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

    63

                reached = position+Vector(deltaX, i);

    64

            } else {

    65

                break;

    66

            }

    67

    68

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

    69

            if (i >= 2) {

    70

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

    71

                this->inAir = true;

    72

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

    73

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

    74

                reached.y -= 2;

    75

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

    76

                break;

    77

            }

    78

        }

    79

    }

    80

    // And we return where we got

    81

    return reached;

    82

}

    83

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

    84

    float velocity = PLAYER_WALK_SPEED;

    85

    float walkAmount = (velocity*dt)/1000;

    86

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

    87

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

    88

        walkAmount--;

    89

    }

    90

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

    91

}

    92

    93

/**

    94

 * Makes the player jump in the air.

    95

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

    96

 */

    97

void PhysicsObject::jump (int direction) {

    98

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

    99

    if (!this->inAir) {

   100

 	    velocity.y = -100;

   101

        switch (direction) {

   102

            case 1:

   103

                velocity.x += 20;

   104

                break;

   105

            case -1:

   106

                velocity.x -= 20;

   107

                break;

   108

            case 0:

   109

                break;

   110

            default:

   111

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

   112

        }

   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.terrain.collides(loc+shape[i]))

   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 (TimeMS dt) {

   130

    // Add gravity to the force queue

   131

    applyForce(world.gravity);

   132

   133

    // If the object (practically player) has a pivot point add

   134

    // a force towards that

   135

    if (pivot != NULL) {

   136

        applyForce(getPivotForce());

   137

   138

        if (pivot->type == PLAYER) {

   139

            pivot->applyForce(-getPivotForce());

   140

        }

   141

    }

   142

   143

    std::pair<Force, TimeMS> force;

   144

    std::queue<std::pair<Force, TimeMS> > newfq;

   145

   146

    Force total;

   147

   148

    while (!forceq.empty()) {

   149

        force = forceq.front();

   150

   151

        if (force.second > dt) {

   152

            force.second -= dt;

   153

            newfq.push(force);

   154

        }

   155

   156

        total += force.first;

   157

        forceq.pop();

   158

    }

   159

   160

    forceq = newfq;

   161

   162

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

   163

    // set inAir false

   164

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

   165

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

   166

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

   167

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

   168

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

   169

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

   170

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

   171

        if (total.y < 0.01 || fabs(total.x) > PLAYER_MOVE_FORCE + 0.1)

   172

            this->inAir = true;

   173

    }

   174

   175

    if (!possibleLocation(position)) {

   176

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

   177

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

   178

        //return;

   179

    }

   180

   181

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

   182

    // otherwise integrate the new position and velocity

   183

    if (!this->inAir) {

   184

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

   185

        if (total.x != 0) {

   186

            walk(dt, total.x > 0);

   187

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

   188

        }   

   189

        // Now the possible walking has been done so we can return from this function.

   190

        // In walk inAir could have been set true, but that will be handled in the next tick.

   191

        return;

   192

    }

   193

   194

    if (!possibleLocation(position))

   195

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

   196

   197

    Vector newPosition;

   198

    Vector velAfterTick;

   199

    // Calculate new position and velocity to the given references

   200

    integrate(total, dt, newPosition, velAfterTick);

   201

    this->velocity = velAfterTick;

   202

   203

    // Collision detection

   204

    bool collided = false;

   205

    Vector collisionPoint;

   206

   207

    const Vector diffVec = newPosition - position;

   208

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

   209

    if (unitVector == Vector(0, 0)) {

   210

        return;

   211

    }

   212

    Vector reached = position;

   213

   214

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

   215

        reached += unitVector;

   216

        // Check if any of the shapes points collide

   217

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

   218

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

   219

   220

                collisionPoint = reached+shape[i];

   221

   222

                if (inAir)

   223

                    this->bounce(world.terrain.getNormal(reached + shape[i], reached - unitVector + shape[i]));

   224

   225

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

   226

                collided = true;

   227

   228

                // snap velocity to zero once it's below a threshold

   229

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

   230

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

   231

   232

                break;

   233

            }

   234

        }

   235

   236

        if (collided)

   237

            break;

   238

    }

   239

   240

   241

    if (!possibleLocation(reached))

   242

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

   243

   244

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

   245

    if (!collided) {

   246

        if (!possibleLocation(newPosition)) {

   247

            newPosition = reached;

   248

   249

        } else {

   250

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

   251

        }

   252

   253

        setPosition(newPosition);

   254

   255

    } else {

   256

        newPosition = reached;

   257

        setPosition(newPosition);

   258

   259

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

   260

        onCollision(collisionPoint);

   261

   262

        return;

   263

    }

   264

}

   265

   266

/**

   267

 * Bounces from straight wall in any direction.

   268

 * Direction given as normal of that wall

   269

 */

   270

void PhysicsObject::bounce (Vector normal) {

   271

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

   272

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

   273

        Vector nvel = velocity;

   274

   275

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

   276

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

   277

        velocity = nvel;

   278

   279

        // We lose some of our speed on collision

   280

        this->velocity *= this->collision_elasticity;

   281

    }

   282

}

   283

   284

/**

   285

 * Integrates given force over time and stores new position to

   286

 * posAfterTick and new velocity to velAfterTick.

   287

 * @param force Force vector.

   288

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

   289

 */

   290

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

   291

    posAfterTick = position;

   292

    velAfterTick = velocity;

   293

   294

    Derivative tmpd;

   295

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

   296

    Derivative k2 = evaluate(force, dt / 2, k1, posAfterTick, velAfterTick);

   297

    Derivative k3 = evaluate(force, dt / 2, k2, posAfterTick, velAfterTick);

   298

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

   299

   300

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

   301

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

   302

   303

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

   304

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

   305

}

   306

   307

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

   308

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

   309

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

   310

   311

    return Derivative(curVel, acceleration(force));

   312

}

   313

   314

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

   315

    return (force / mass);

   316

}

   317

   318

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

   319

    // Add applied force to the queue

   320

    forceq.push(std::make_pair(force, dt));

   321

}

   322

   323

void PhysicsObject::changeAim(float da) {

   324

    this->aim += da;

   325

   326

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

   327

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

   328

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

   329

}

   330

   331

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

   332

    setPosition (position);

   333

    this->velocity = velocity;

   334

    this->inAir = inAir;

   335

    this->facing = facing;

   336

    this->aim = aim;

   337

}

   338

   339

PixelCoordinate PhysicsObject::getCoordinate (void) const {

   340

    return world.terrain.getPixelCoordinate(position);

   341

}

   342

   343

Vector PhysicsObject::getDirection (void) const {

   344

    return facing == FACING_RIGHT ? Vector(cos(aim), -sin(aim)) : Vector(-cos(aim), -sin(aim));

   345

}

   346

   347

void PhysicsObject::tick (TimeMS tick_length) {

   348

    this->updatePosition(tick_length);

   349

}

   350

   351

void PhysicsObject::enable (void) {

   352

    // only enable once until disabled

   353

    if (alive)

   354

        return;

   355

   356

    // mark as alive

   357

    alive = true;

   358

   359

    // add the world objects list

   360

    world.addPhysicsObject(this);

   361

}

   362

   363

void PhysicsObject::disable (void) {

   364

    // mark as disabled

   365

    alive = false;

   366

}

   367

   368

void PhysicsObject::destroy (void) {

   369

    // mark as disabled and for deletion

   370

    alive = false;

   371

    shouldDelete = true;

   372

}

   373

   374

bool PhysicsObject::removeIfDestroyed (void) {

   375

    if (!alive) {

   376

        if (shouldDelete)

   377

            delete this;

   378

   379

        return true;

   380

   381

    } else {

   382

        return false;

   383

    }

   384

}

   385

   386

Vector PhysicsObject::getPivotForce (void) { 

   387

    return Vector(0,0); 

   388

}

   389

   390

bool PhysicsObject::collides (const PhysicsObject &obj) {

   391

    const std::vector<Vector> oShape = obj.shape;

   392

    Vector p1, p2, p3;

   393

    int8_t sign, nsign;

   394

    for (std::vector<Vector>::const_iterator i = oShape.begin(); i != oShape.end(); i++) { // For every point in other shape

   395

        p3 = *i + obj.getPosition();

   396

        sign = 0;

   397

        for (std::vector<Vector>::const_iterator j = shape.begin(); j != shape.end(); j++) {

   398

            p1 = *j + position;

   399

            if ( (j+1) == shape.end() ) p2 = *shape.begin() + position;

   400

            else p2 = *(j+1) + position;

   401

            nsign = crossProduct(p1, p2, p3);

   402

            if ( sign == 0 ) sign = nsign;

   403

            else if ( ((sign < 0) && (nsign < 0)) || ((sign > 0) && (nsign > 0)) ) continue;

   404

            else return false;

   405

        }

   406

    }

   407

    return true;

   408

}

   409

   410

void  PhysicsObject::onCollision (Vector collisionPoint, PhysicsObject *other) {

   411

    (void) collisionPoint;

   412

    (void) other;

   413

}

   414

   415

int8_t crossProduct (const Vector &p1, const Vector &p2, const Vector &p3) {

   416

    float p = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);

   417

    if (p < 0)

   418

        return -1;

   419

    else

   420

        return 1;

   421

}

   422

   423

void PhysicsObject::setPosition (Vector pos) {

   424

    this->previousPosition = this->position;

   425

    this->position = pos;

   426

}

   427

   428

void PhysicsObject::reset (void) {

   429

    // zero velocity

   430

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

   431

   432

    // disable

   433

    disable();

   434

}

   436

void PhysicsObject::resume (Vector position) {

   437

    // update position

   438

    setPosition(position);

   439

   440

    // enable again

   441

    enable();

   442

}