src/phisics.cpp

   1 /*
   2  * Copyright 2008 Jacek Caban
   3  * Copyright 2008 Piotr Caban
   4  * Copyright 2008 Jaroslaw Sobiecki
   5  *
   6  * This program is free software: you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation, either version 3 of the License, or
   9  * (aU your option) any later version.
  10
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <sys/time.h>
  22
  23 #include <Ogre.h>
  24
  25 #include "phisics.h"
  26
  27 using namespace Ogre;
  28
  29 enum {
  30     BOUND_LEFT,
  31     BOUND_RIGHT,
  32     BOUND_BOTTOM,
  33     BOUND_UPPER
  34 };
  35
  36 //#define DEBUG_PHISICS
  37
  38 #ifdef DEBUG_PHISICS
  39
  40 #define trace printf
  41
  42 #else
  43
  44 #define trace(...)
  45
  46 #endif
  47
  48 #define trace_vector(v) trace(#v ": %lf %lf %lf\n", (v)[0], (v)[1], (v)[2])
  49 #define trace_quaternion(v) trace(#v ": %lf %lf %lf %lf\n", (v).w, (v).x, (v).y, (v).z)
  50
  51 const TableParameters TableParameters::DefaultTableParameters;
  52
  53 #define MAX_DT 0.01
  54 #define ALMOST_ZERO 0.0001
  55
  56 static Ogre::Real inline deg2rad(Ogre::Real alpha) {
  57     return (alpha/360.0)*2.0*M_PI;
  58 }
  59
  60 /**
  61  * Constructor of the Table class. Sets table and phisics parameters.
  62  */
  63 Table::Table(Real sx, Real sy, const TableParameters *p) : size_x(sx/2.0), size_y(sy/2.0), current_time(0), params(p)
  64 {
  65     gettimeofday(&start_time, NULL);
  66 }
  67
  68 /**
  69  * Updates state od balls diring calculated dt (small delta time).
  70  */
  71 Real Table::eulerUpdate() {
  72     BallVector::iterator iter, iter2;
  73     Real dt = MAX_DT, tmp;
  74     Ball *ball = NULL, *ball2;
  75     int bound, bound_copy;
  76
  77     enum {
  78         BALL_NONE,
  79         BALL_BOUND,
  80         BALL_BALL
  81     } action_type = BALL_NONE;
  82
  83     trace("eulerUpdate\n");
  84
  85     /* Find the next action */
  86     for(iter = balls.begin(); iter != balls.end(); iter++) {
  87         tmp = boundCrossTime(**iter, bound);
  88         if(tmp < dt) {
  89             action_type = BALL_BOUND;
  90             ball = *iter;
  91             bound_copy = bound;
  92             dt = tmp;
  93         }
  94
  95         for(iter2 = iter+1; iter2 != balls.end(); iter2++) {
  96             tmp = ballCrossTime(**iter, **iter2);
  97             if(tmp < dt) {
  98                 action_type = BALL_BALL;
  99                 ball = *iter;
 100                 ball2 = *iter2;
 101                 dt = tmp;
 102             }
 103         }
 104     }
 105
 106     trace("dt=%lf (%d)\n", dt, action_type);
 107
 108     simpleUpdate(dt);
 109
 110     switch(action_type) {
 111     case BALL_NONE:
 112         break;
 113     case BALL_BOUND:
 114         boundCross(*ball, bound_copy);
 115         break;
 116     case BALL_BALL:
 117         ballCross(*ball, *ball2);
 118         break;
 119     }
 120
 121     return dt;
 122 }
 123
 124 /**
 125  * Updates balls state depending on their states.
 126  */
 127 void Table::simpleUpdate(Real dt) {
 128     BallVector::iterator iter;
 129     Vector3 norm, r = Vector3::ZERO, tmp, rotv, rotp, slow;
 130     Ball *ball;
 131
 132     /* Slow down all balls (resistance from the table) */
 133     for(iter = balls.begin(); iter != balls.end(); iter++) {
 134         ball = *iter;
 135         norm = ball->speed.normalisedCopy();
 136         r[2] = -ball->r;
 137
 138         trace("preUpdate:\n");
 139         trace_vector(ball->position);
 140         trace_vector(ball->speed);
 141         trace_vector(ball->rotation);
 142         trace_quaternion(ball->orientation);
 143
 144         //Set ball orientation
 145         ball->orientation = Quaternion(Radian(ball->rotation.length()*dt), ball->rotation.normalisedCopy())
 146             * ball->orientation;
 147
 148         ball->position += ball->speed*dt;
 149         tmp = params->slidingFriction * params->gravity * dt * norm;
 150         if(ball->speed.squaredLength() > tmp.squaredLength())
 151             ball->speed -= tmp;
 152         else
 153             ball->speed = Vector3::ZERO;
 154
 155         tmp[0] = norm[1];
 156         tmp[1] = -norm[0];
 157         tmp[2] = 0.0;
 158         rotv = ball->rotation.dotProduct(tmp)*tmp;
 159         rotp = ball->rotation - rotv;
 160         tmp = r.crossProduct(rotv);
 161         slow = 10*Vector3::UNIT_Z.crossProduct(norm)*params->staticFriction*dt;
 162         trace_vector(rotv);
 163         trace_vector(rotp);
 164         trace_vector(tmp);
 165         trace_vector(slow);
 166         if(tmp.dotProduct(rotv) < 0.0 || tmp.squaredLength() < ball->speed.squaredLength()) {
 167             trace("rotup\n");
 168             rotv += slow;
 169         }else {
 170             trace("rotdown\n");
 171             rotv -= slow;
 172         }
 173
 174         rotp -= rotp.normalisedCopy()*params->staticFriction*dt;
 175         ball->rotation = rotv+rotp;
 176
 177         tmp = ball->rotation;
 178         tmp[2] = 0.0;
 179         tmp.normalise();
 180         ball->speed += -dt*0.01*params->staticFriction*tmp.crossProduct(r);
 181
 182         trace("simpleUpdate:\n");
 183         trace_vector(ball->position);
 184         trace_vector(ball->speed);
 185         trace_vector(ball->rotation);
 186     }
 187 }
 188
 189 /**
 190  * Sets the ball to be fallen.
 191  */
 192 inline void Table::ballFallIn(Ball &ball) {
 193     ball.position.z = -10.666;
 194     ball.onTable = false;
 195     ball.speed = ball.rotation = Vector3::ZERO;
 196 }
 197
 198 /**
 199  * Handles crossing ball and bound event.
 200  */
 201 void Table::boundCross(Ball &ball, int bound) {
 202     Real rot = ball.rotation.z*params->boundFriction*ball.speed.length();
 203
 204     if((ball.position-Vector3(size_x, size_y, 0.6)).squaredLength() < params->pocketSize) {
 205         ballFallIn(ball);
 206         return;
 207     }
 208     else if((ball.position-Vector3(size_x, -size_y, 0.6)).squaredLength() < params->pocketSize) {
 209         ballFallIn(ball);
 210         return;
 211     }
 212     else if((ball.position-Vector3(0.0, size_y, 0.6)).squaredLength() < params->pocketSize) {
 213         ballFallIn(ball);
 214         return;
 215     }
 216     else if((ball.position-Vector3(0.0, -size_y, 0.6)).squaredLength() < params->pocketSize) {
 217         ballFallIn(ball);
 218         return;
 219     }
 220     else if((ball.position-Vector3(-size_x, size_y, 0.6)).squaredLength() < params->pocketSize) {
 221         ballFallIn(ball);
 222         return;
 223     }
 224     else if((ball.position-Vector3(-size_x, -size_y, 0.6)).squaredLength() < params->pocketSize) {
 225         ballFallIn(ball);
 226         return;
 227     }
 228
 229     trace("boundCross\n");
 230
 231     switch(bound) {
 232     case BOUND_LEFT:
 233         ball.speed[0] = -ball.speed[0];
 234         ball.speed[1] += rot;
 235         break;
 236     case BOUND_RIGHT:
 237         ball.speed[0] = -ball.speed[0];
 238         ball.speed[1] -= rot;
 239         break;
 240     case BOUND_BOTTOM:
 241         ball.speed[0] -= rot;
 242         ball.speed[1] = -ball.speed[1];
 243         break;
 244     case BOUND_UPPER:
 245         ball.speed[0] += rot;
 246         ball.speed[1] = -ball.speed[1];
 247         break;
 248     }
 249
 250     ball.speed *= params->boundResistance;
 251     ball.rotation[2] = ALMOST_ZERO;
 252     ball.rotation *= 0.3;
 253 }
 254
 255 /**
 256  * Handles two balls crossing event.
 257  */
 258 void Table::ballCross(Ball &ball1, Ball &ball2) {
 259     Vector3 r, norm, v1x, v2x, v1y, v2y;
 260
 261     if(!ball1.firstHit) ball1.firstHit = ball2.ballNo;
 262     if(!ball2.firstHit) ball2.firstHit = ball1.ballNo;
 263
 264     trace("preBallCross:\n");
 265     trace_vector(ball1.position);
 266     trace_vector(ball1.speed);
 267     trace_vector(ball1.rotation);
 268     trace_vector(ball2.position);
 269     trace_vector(ball2.speed);
 270     trace_vector(ball2.rotation);
 271
 272     r = ball1.position-ball2.position;
 273     norm[0] = r[1];
 274     norm[1] = -r[0];
 275     norm[2] = 0.0;
 276     norm.normalise();
 277     r *= 0.5;
 278
 279     v1x = ball1.speed.dotProduct(norm) * norm;
 280     v2x = ball2.speed.dotProduct(norm) * norm;
 281     v1y = ball1.speed - v1x;
 282     v2y = ball2.speed - v2x;
 283
 284     trace_vector(v1x);
 285     trace_vector(v2x);
 286     trace_vector(v1y);
 287     trace_vector(v2y);
 288
 289     ball1.speed = v1x+v2y;
 290     ball2.speed = v2x+v1y;
 291
 292     trace("ballCross:\n");
 293     trace_vector(ball1.position);
 294     trace_vector(ball1.speed);
 295     trace_vector(ball1.rotation);
 296     trace_vector(ball2.position);
 297     trace_vector(ball2.speed);
 298     trace_vector(ball2.rotation);
 299 }
 300
 301 /**
 302  * Calculates time to the nearest ball and bound cross.
 303  */
 304 Real Table::boundCrossTime(Ball &ball, int &bound) {
 305     Real ret, tmp;
 306
 307     trace("boundCrossTime:\n");
 308     if(ball.speed[0] < -ALMOST_ZERO) {
 309         bound = BOUND_LEFT;
 310         ret = (-(size_x-ball.r) - ball.position[0]) / ball.speed[0];
 311         trace("left %lf\n", ret);
 312     }else if(ball.speed[0] > ALMOST_ZERO) {
 313         bound = BOUND_RIGHT;
 314         ret = (size_x-ball.r - ball.position[0]) / ball.speed[0];
 315         trace("right %lf\n", ret);
 316     }else {
 317         trace("left right no\n");
 318         ret = 100.0;
 319     }
 320
 321     if(ball.speed[1] < -ALMOST_ZERO) {
 322         tmp = (-(size_y-ball.r) - ball.position[1]) / ball.speed[1];
 323         trace("bottom %lf\n", tmp);
 324         if(tmp < ret) {
 325             bound = BOUND_BOTTOM;
 326             ret = tmp;
 327         }
 328     }else if(ball.speed[1] > ALMOST_ZERO) {
 329         tmp = (size_y-ball.r - ball.position[1]) / ball.speed[1];
 330         trace("upper %lf\n", tmp);
 331         if(tmp < ret) {
 332             bound = BOUND_UPPER;
 333             ret = tmp;
 334         }
 335     }
 336
 337     if(ret == 100.0)
 338         return ret;
 339
 340     trace_vector(ball.position);
 341     trace_vector(ball.speed);
 342     trace_vector(ball.rotation);
 343     trace("ret %lf\n", ret);
 344
 345     if(ret < 0.0)
 346         ret = 0.0;
 347
 348     return ret;
 349 }
 350
 351 /**
 352  * Calculates time to the nearest two balls cross.
 353  */
 354 Real Table::ballCrossTime(Ball &ball1, Ball &ball2) {
 355     Vector3 dp, dp_next;
 356     Ball ball1_next = ball1;
 357     Ball ball2_next = ball2;
 358
 359     ball1_next.position.x += ball1.speed.x*ALMOST_ZERO;
 360     ball1_next.position.y += ball1.speed.y*ALMOST_ZERO;
 361     ball2_next.position.x += ball2.speed.x*ALMOST_ZERO;
 362     ball2_next.position.y += ball2.speed.y*ALMOST_ZERO;
 363
 364     dp = ball2.position - ball1.position;
 365     dp_next = ball2_next.position - ball1_next.position;
 366
 367     if(dp.squaredLength()<dp_next.squaredLength()+ALMOST_ZERO*ALMOST_ZERO) return 100.0;
 368     if(dp.squaredLength()>4.0*ball1.r) return 100.0;
 369
 370     return ALMOST_ZERO;
 371 }
 372
 373 /**
 374  * Checks if there is any ball moving.
 375  */
 376 bool Table::ballsMoving() {
 377     BallVector::iterator iter;
 378     Ball *ball;
 379
 380     for(iter = balls.begin(); iter != balls.end(); iter++) {
 381         ball = *iter;
 382
 383         if(ball->speed.squaredLength() > ALMOST_ZERO)
 384             return true;
 385
 386         ball->speed = ball->rotation = Vector3::ZERO;
 387     }
 388
 389     return false;
 390 }
 391
 392 static unsigned timeval2ms(const struct timeval &t) {
 393     return t.tv_sec*1000 + t.tv_usec/1000;
 394 }
 395
 396 /**
 397  * Updates balls state to the given time.
 398  */
 399 bool Table::update(Real time) {
 400     trace("update >\n");
 401
 402     current_time += time;
 403
 404     while(current_time>0.0) {
 405         if(!ballsMoving()) {
 406             trace("update <<\n");
 407             return false;
 408         }
 409
 410         current_time -= eulerUpdate()/2.0;
 411     }
 412
 413     return true;
 414 }
 415
 416 /**
 417  * Handles shotting ball by cue.
 418  */
 419 void Ball::shot(Real direction, Real force, Vector3 shot_pos, Real forceScale, Real hitTime) {
 420     Vector3 vr, dir;
 421
 422     direction = deg2rad(direction);
 423     shot_pos[0] = deg2rad(shot_pos[0]);
 424     shot_pos[1] = deg2rad(shot_pos[1]);
 425     shot_pos[2] = deg2rad(shot_pos[2]);
 426
 427     dir[0] = -Math::Cos(direction);
 428     dir[1] = -Math::Sin(direction);
 429     dir[2] = 0;
 430
 431     trace("shot:   %lf %lf\n", direction, force);
 432     trace_vector(shot_pos);
 433
 434     force *= forceScale;
 435
 436     vr[0] = Math::Cos(direction + shot_pos[0]);
 437     vr[1] = Math::Sin(direction + shot_pos[0]);
 438     vr[2] = Math::Sin(shot_pos[1]);
 439     vr.normalise();
 440     vr *= r;
 441
 442     /* v' = (-k dt)/m * x */
 443     speed = hitTime/mass*dir*force;
 444
 445     dir *= -r;
 446
 447     /* w' = 2.5 * dt / (m r2) (r x (-kx)) */
 448     rotation = 2.5 * (20.0 * hitTime / (mass * r*r)) * (vr.crossProduct(dir));
 449
 450     trace_vector(position);
 451     trace_vector(speed);
 452     trace_vector(rotation);
 453 }