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

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

    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

    Vector reached = this->position;

    39

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

    40

        return reached+Vector(deltaX, 0);

    41

    }

    42

    // Is there upward ramp

    43

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

    44

        // Yes. Then we check n pixels up

    45

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

    46

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

    47

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

    48

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

    49

                break;

    50

            }

    51

        }

    52

    } else {

    53

        // Or downward ramp or flat

    54

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

    55

    56

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

    57

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

    58

            // but will fall one pixel down

    59

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

    60

                reached = position+Vector(deltaX, i);

    61

            } else {

    62

                break;

    63

            }

    64

    65

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

    66

            if (i >= 2) {

    67

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

    68

                this->inAir = true;

    69

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

    70

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

    71

                reached.y -= 2;

    72

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

    73

                break;

    74

            }

    75

        }

    76

    }

    77

    // And we return where we got

    78

    return reached;

    79

}

    80

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

    81

    float velocity = PLAYER_WALK_SPEED;

    82

    float walkAmount = (velocity*dt)/1000;

    83

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

    84

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

    85

        walkAmount--;

    86

    }

    87

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

    88

}

    89

    90

/**

    91

 * Makes the player jump in the air.

    92

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

    93

 */

    94

void PhysicsObject::jump (int direction) {

    95

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

    96

    if (!this->inAir) {

    97

 	    velocity.y = -100;

    98

        switch (direction) {

    99

            case 1:

   100

                velocity.x += 20;

   101

                break;

   102

            case -1:

   103

                velocity.x -= 20;

   104

                break;

   105

            case 0:

   106

                break;

   107

            default:

   108

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

   109

        }

   110

	    inAir = true;

   111

    }

   112

}

   113

   114

bool PhysicsObject::possibleLocation (Vector loc) {

   115

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

   116

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

   117

            return false;

   118

    }

   119

    return true;

   120

}

   121

   122

/**

   123

 * Updates object speed and position. This function organises force

   124

 * integration and collision detection.

   125

 */   

   126

void PhysicsObject::updatePosition (TimeMS dt) {

   127

    // Add gravity to the force queue

   128

    applyForce(world.gravity);

   129

   130

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

   131

    // a force towards that

   132

    if (pivot != NULL) {

   133

        applyForce(getPivotForce());

   137

    }

   138

   139

    std::pair<Force, TimeMS> force;

   140

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

   141

    Force total;

   142

    while (!forceq.empty()) {

   143

        force = forceq.front();

   144

        if (force.second > dt) {

   145

            force.second -= dt;

   146

            newfq.push(force);

   147

        }

   148

        total += force.first;

   149

        forceq.pop();

   150

    }

   151

    forceq = newfq;

   152

   153

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

   154

    // set inAir false

   155

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

   156

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

   157

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

   158

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

   159

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

   160

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

   161

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

   162

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

   163

            this->inAir = true;

   164

    }

   165

   166

    if(!possibleLocation(position)) {

   167

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

   168

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

   169

        //return;

   170

    }

   171

   172

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

   173

    // otherwise integrate the new position and velocity

   174

    if (!this->inAir) {

   175

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

   176

        if (total.x != 0) {

   177

            walk(dt, total.x > 0);

   178

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

   179

        }   

   180

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

   181

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

   182

        return;

   183

    }

   184

   185

    if (!possibleLocation(position))

   186

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

   187

   188

    Vector newPosition;

   189

    Vector velAfterTick;

   190

    // Calculate new position and velocity to the given references

   191

    integrate(total, dt, newPosition, velAfterTick);

   192

    this->velocity = velAfterTick;

   193

   194

    // Collision detection

   195

    bool collided = false;

   196

    Vector collisionPoint;

   197

   198

    const Vector diffVec = newPosition - position;

   199

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

   200

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

   201

        return;

   202

    }

   203

    Vector reached = position;

   204

   205

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

   206

        reached += unitVector;

   207

        // Check if any of the shapes points collide

   208

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

   209

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

   210

   211

                collisionPoint = reached+shape[i];

   212

   213

                if (inAir)

   214

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

   215

   216

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

   217

                collided = true;

   218

   219

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

   220

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

   221

   222

                break;

   223

            }

   224

        }

   225

   226

        if (collided)

   227

            break;

   228

    }

   229

   230

   231

    if (!possibleLocation(reached))

   232

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

   233

   234

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

   235

    if (!collided) {

   236

        if (!possibleLocation(newPosition)) {

   237

            newPosition = reached;

   238

        } else {

   239

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

   240

        }

   241

   242

        setPosition (newPosition);

   243

   244

    } else {

   245

        newPosition = reached;

   246

        setPosition (newPosition);

   247

   248

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

   249

        onCollision(collisionPoint);

   250

   251

        return;

   252

    }

   253

}

   254

   255

/**

   256

 * Bounces from straight wall in any direction.

   257

 * Direction given as normal of that wall

   258

 */

   259

void PhysicsObject::bounce (Vector normal) {

   260

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

   261

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

   262

        Vector nvel = velocity;

   263

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

   264

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

   265

        velocity = nvel;

   266

        // We lose some of our speed on collision

   267

        this->velocity *= this->collision_elasticity;

   268

    }

   269

}

   270

   271

/**

   272

 * Integrates given force over time and stores new position to

   273

 * posAfterTick and new velocity to velAfterTick.

   274

 * @param force Force vector.

   275

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

   276

 */

   277

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

   278

    posAfterTick = position;

   279

    velAfterTick = velocity;

   280

    Derivative tmpd;

   281

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

   282

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

   283

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

   284

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

   285

   286

   287

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

   288

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

   289

   290

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

   291

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

   292

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

   293

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

   294

}

   295

   296

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

   297

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

   298

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

   299

   300

    Derivative out;

   301

    out.dx = curVel;

   302

    out.dv = acceleration(force);

   303

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

   304

    return out;

   305

}

   306

   307

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

   308

    return (force/mass);

   309

}

   310

   311

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

   312

    // Add applied force to the queue

   313

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

   314

}

   315

   316

void PhysicsObject::changeAim(float da) {

   317

    this->aim += da;

   318

   319

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

   320

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

   321

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

   322

}

   323

   324

ObjectType PhysicsObject::getType (void) const {

   325

    return this->type;

   326

}

   327

   328

void PhysicsObject::setFacing (FacingDirection facing) {

   329

    this->facing = facing;

   330

}

   331

   332

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

   333

    setPosition (position);

   334

    this->velocity = velocity;

   335

    this->inAir = inAir;

   336

    this->facing = facing;

   337

    this->aim = aim;

   338

}

   339

   340

Vector PhysicsObject::getPosition (void) const {

   341

    return position;

   342

}

   343

   344

Vector PhysicsObject::getPreviousPosition (void) const {

   345

    return previousPosition;

   346

}

   347

   348

void PhysicsObject::setPosition (Vector pos) {

   349

    this->previousPosition = this->position;

   350

    this->position = pos;

   351

}

   352

   353

PixelCoordinate PhysicsObject::getCoordinate (void) const {

   354

    return world.getPixelCoordinate(position);

   355

}

   356

   357

Vector PhysicsObject::getVelocity (void) const {

   358

    return velocity;

   359

}

   360

   361

FacingDirection PhysicsObject::getFacing (void) const {

   362

    return facing;

   363

}

   364

   365

float PhysicsObject::getAim (void) const {

   366

    return aim;

   367

}

   368

   369

Vector PhysicsObject::getDirection (void) const {

   370

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

   371

}

   372

   373

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

   374

    return shape;

   375

}

   376

   377

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

   378

    this->shape = shape;

   379

}

   380

   381

PhysicsObject *PhysicsObject::getPivot (void) {

   382

    return this->pivot;

   383

}

   384

   385

void PhysicsObject::setPivot (PhysicsObject *pivot) {

   386

    this->pivot = pivot;

   387

}

   388

   389

void PhysicsObject::tick (TimeMS tick_length) {

   390

    this->updatePosition(tick_length);

   391

}

   392

   393

void PhysicsObject::enable (void) {

   394

    // only enable once until disabled

   395

    if (alive)

   396

        return;

   397

   398

    // mark as alive

   399

    alive = true;

   400

   401

    // add the world objects list

   402

    world.addPhysicsObject(this);

   403

}

   404

   405

void PhysicsObject::disable (void) {

   406

    // mark as disabled

   407

    alive = false;

   408

}

   409

   410

void PhysicsObject::destroy (void) {

   411

    // mark as disabled and for deletion

   412

    alive = false;

   413

    shouldDelete = true;

   414

}

   415

   416

bool PhysicsObject::isAlive (void) {

   417

    return alive;

   418

}

   419

   420

bool PhysicsObject::removeIfDestroyed (void) {

   421

    if (!alive) {

   422

        if (shouldDelete)

   423

            delete this;

   424

   425

        return true;

   426

   427

    } else {

   428

        return false;

   429

    }

   430

}

   431

   432

Vector PhysicsObject::getPivotForce (void) { 

   433

    return Vector(0,0); 

   434

}

   435

   436

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

   437

    const std::vector<Vector> oShape = obj.getShape();

   438

    Vector p1, p2, p3;

   439

    int8_t sign, nsign;

   440

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

   441

        p3 = *i + obj.getPosition();

   442

        sign = 0;

   443

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

   444

            p1 = *j + position;

   445

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

   446

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

   447

            nsign = crossProduct(p1, p2, p3);

   448

            if ( sign == 0 ) sign = nsign;

   449

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

   450

            else return false;

   451

        }

   452

    }

   453

    return true;

   454

}

   455

   456

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

   457

    (void) collisionPoint;

   458

    (void) other;

   459

}

   460

   461

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

   462

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

   463

    if (p < 0)

   464

        return -1;

   465

    else

   466

        return 1;

   467

}