git.cworth.org Git - akamaru/blob - akamaru.c

   1 /*                                           -*- mode: c; c-basic-offset: 8 -*-
   2  * See:
   3  *
   4  *     http://en.wikipedia.org/wiki/Verlet_integration
   5  *     http://www.teknikus.dk/tj/gdc2001.htm
   6  *
   7  * TODO:
   8  *
   9  * - Add code to add boxes
  10  * - Add circle object
  11  * - Try out this idea: make constraint solver take mean of all
  12  *   corrections at the end instead of meaning as it goes.
  13  */
  14
  15 #include <glib.h>
  16 #include <stdlib.h>
  17 #include <string.h>
  18 #include <stdarg.h>
  19 #include <math.h>
  20
  21 #include "akamaru.h"
  22
  23 const double elasticity = 0.7;
  24 const double friction = 8;
  25 const double gravity = 50;
  26
  27 void
  28 object_init (Object *object, double x, double y, double mass)
  29 {
  30         object->position.x = x;
  31         object->position.y = y;
  32         object->previous_position.x = x;
  33         object->previous_position.y = y;
  34         object->mass = mass;
  35 }
  36
  37 void
  38 spring_init (Spring *spring, Object *a, Object *b, double length)
  39 {
  40         spring->a = a;
  41         spring->b = b;
  42         spring->length = length;
  43 }
  44
  45 void
  46 stick_init (Stick *stick, Object *a, Object *b, double length)
  47 {
  48         stick->a = a;
  49         stick->b = b;
  50         stick->length = length;
  51 }
  52
  53 void
  54 string_init (String *string, Object *a, Object *b, double length)
  55 {
  56         string->a = a;
  57         string->b = b;
  58         string->length = length;
  59 }
  60
  61 void
  62 offset_spring_init (OffsetSpring *spring, Object *a, Object *b,
  63                     double dx, double dy)
  64 {
  65         spring->a = a;
  66         spring->b = b;
  67         spring->dx = dx;
  68         spring->dy = dy;
  69 }
  70
  71 void
  72 spacer_init (Spacer *spacer, Object *a, Object *b, double length)
  73 {
  74         spacer->a = a;
  75         spacer->b = b;
  76         spacer->length = length;
  77 }
  78
  79 void
  80 anchor_init (Anchor *anchor, Object *object, double x, double y)
  81 {
  82         anchor->object = object;
  83         anchor->x = x;
  84         anchor->y = y;
  85 }
  86
  87 void
  88 polygon_init (Polygon *p, int enclosing, int num_points, ...)
  89 {
  90         double dx, dy, length;
  91         int i, j;
  92         va_list ap;
  93
  94         /* Polygons are defined counter-clock-wise in a coordinate system
  95          * with the y-axis pointing down. */
  96
  97         va_start (ap, num_points);
  98         p->num_points = num_points;
  99         p->points = g_new (Point, num_points);
 100         p->enclosing = enclosing;
 101
 102         for (i = 0; i < num_points; i++) {
 103                 p->points[i].x = va_arg (ap, double);
 104                 p->points[i].y = va_arg (ap, double);
 105         }
 106         va_end (ap);
 107
 108         p->normals = g_new (Vector, p->num_points);
 109         /* Compute outward pointing normals.  p->normals[i] is the normal
 110          * for the edged between p->points[i] and p->points[i + 1]. */
 111         for (i = 0; i < p->num_points; i++) {
 112                 j = (i + 1) % p->num_points;
 113                 dx = p->points[j].x - p->points[i].x;
 114                 dy = p->points[j].y - p->points[i].y;
 115                 length = sqrt (dx * dx + dy * dy);
 116                 p->normals[i].x = -dy / length;
 117                 p->normals[i].y = dx / length;
 118         }
 119 }
 120
 121 void
 122 polygon_init_diamond (Polygon *polygon, double x, double y)
 123 {
 124         return polygon_init (polygon, FALSE, 5,
 125                              x, y,
 126                              x + 10, y + 40,
 127                              x + 90, y + 40,
 128                              x + 100, y,
 129                              x + 50, y - 20);
 130 }
 131
 132 void
 133 polygon_init_rectangle (Polygon *polygon, double x0, double y0,
 134                         double x1, double y1)
 135 {
 136         return polygon_init (polygon, FALSE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
 137 }
 138
 139 void
 140 polygon_init_enclosing_rectangle (Polygon *polygon, double x0, double y0,
 141                                   double x1, double y1)
 142 {
 143         return polygon_init (polygon, TRUE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
 144 }
 145
 146 void
 147 model_fini (Model *model)
 148 {
 149         int i;
 150
 151         g_free (model->objects);
 152         g_free (model->sticks);
 153         g_free (model->strings);
 154         for (i = 0; i < model->num_offsets; i++)
 155                 g_free (model->offsets[i].objects);
 156         g_free (model->springs);
 157         g_free (model->offset_springs);
 158         g_free (model->spacers);
 159         for (i = 0; i < model->num_polygons; i++)
 160                 g_free (model->polygons[i].points);
 161         g_free (model->polygons);
 162
 163         memset (model, 0, sizeof *model);
 164 }
 165
 166 static void
 167 model_accumulate_forces (Model *model)
 168 {
 169         int i;
 170         double x, y, dx, dy, distance, displacement;
 171         Point middle;
 172         Vector u, v;
 173
 174         for (i = 0; i < model->num_objects; i++) {
 175                 /* Gravity */
 176                 model->objects[i].force.x = 0;
 177                 model->objects[i].force.y = gravity * model->objects[i].mass;
 178
 179                 /* Friction */
 180                 v.x = model->objects[i].position.x - model->objects[i].previous_position.x;
 181                 v.y = model->objects[i].position.y - model->objects[i].previous_position.y;
 182                 model->objects[i].force.x -= v.x * friction;
 183                 model->objects[i].force.y -= v.y * friction;
 184         }
 185
 186         for (i = 0; i < model->num_springs; i++) {
 187                 x = model->springs[i].a->position.x;
 188                 y = model->springs[i].a->position.y;
 189                 dx = model->springs[i].b->position.x - x;
 190                 dy = model->springs[i].b->position.y - y;
 191                 distance = sqrt (dx * dx + dy * dy);
 192                 u.x = dx / distance;
 193                 u.y = dy / distance;
 194                 displacement = distance - model->springs[i].length;
 195                 model->springs[i].a->force.x += u.x * model->k * displacement;
 196                 model->springs[i].a->force.y += u.y * model->k * displacement;
 197                 model->springs[i].b->force.x -= u.x * model->k * displacement;
 198                 model->springs[i].b->force.y -= u.y * model->k * displacement;
 199         }
 200
 201         for (i = 0; i < model->num_offset_springs; i++) {
 202                 middle.x =
 203                         (model->offset_springs[i].a->position.x +
 204                          model->offset_springs[i].b->position.x) / 2;
 205                 middle.y =
 206                         (model->offset_springs[i].a->position.y +
 207                          model->offset_springs[i].b->position.y) / 2;
 208
 209                 x = middle.x - model->offset_springs[i].dx / 2;
 210                 y = middle.y - model->offset_springs[i].dy / 2;
 211
 212                 dx = x - model->offset_springs[i].a->position.x;
 213                 dy = y - model->offset_springs[i].a->position.y;
 214
 215                 model->offset_springs[i].a->force.x += dx * model->k;
 216                 model->offset_springs[i].a->force.y += dy * model->k;
 217                 model->offset_springs[i].b->force.x -= dx * model->k;
 218                 model->offset_springs[i].b->force.y -= dy * model->k;
 219         }
 220
 221         for (i = 0; i < model->num_objects; i++) {
 222                 double f =
 223                         model->objects[i].force.x * model->objects[i].force.x +
 224                         model->objects[i].force.y * model->objects[i].force.y;
 225
 226                 if (f > 100000000)
 227                         abort();
 228         }
 229 }
 230
 231 static void
 232 model_integrate (Model *model, double step)
 233 {
 234         double x, y;
 235         Object *o;
 236         int i;
 237
 238         for (i = 0; i < model->num_objects; i++) {
 239                 o = &model->objects[i];
 240                 x = o->position.x;
 241                 y = o->position.y;
 242
 243                 o->position.x =
 244                         x + (x - o->previous_position.x) + o->force.x * step * step;
 245                 o->position.y =
 246                         y + (y - o->previous_position.y) + o->force.y * step * step;
 247
 248                 o->previous_position.x = x;
 249                 o->previous_position.y = y;
 250         }
 251 }
 252
 253 /* The square root in the distance computation for the string and
 254  * stick constraints can be aproximated using Newton:
 255  *
 256  *    distance =
 257  *      (model->sticks[i].length +
 258  *       (dx * dx + dy * dy) / model->sticks[i].length) / 2;
 259  *
 260  * This works really well, since the constraints aren't typically
 261  * violated much.  Thus, the distance is really close to the stick
 262  * length, which then makes a good initial guess.  However, the
 263  * approximation seems to be slower that just calling sqrt()...
 264  */
 265
 266 static inline double
 267 estimate_distance (double dx, double dy, double r)
 268 {
 269 #ifdef APPROXIMATE_SQUARE_ROOTS
 270         return (r + (dx * dx + dy * dy) / r) / 2;
 271 #else
 272         return sqrt (dx * dx + dy * dy);
 273 #endif
 274 }
 275
 276 static int
 277 polygon_contains_point (Polygon *polygon, Point *point)
 278 {
 279         int i;
 280         double dx, dy;
 281
 282         for (i = 0; i < polygon->num_points; i++) {
 283                 dx = point->x - polygon->points[i].x;
 284                 dy = point->y - polygon->points[i].y;
 285
 286                 if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
 287                         return polygon->enclosing;
 288         }
 289
 290         return !polygon->enclosing;
 291 }
 292
 293 static void
 294 polygon_reflect_object (Polygon *polygon, Object *object)
 295 {
 296         int i, edge;
 297         double d, distance;
 298         Vector *n;
 299
 300         distance = -1000;
 301         for (i = 0; i < polygon->num_points; i++) {
 302                 d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
 303                         polygon->normals[i].y * (object->position.y - polygon->points[i].y);
 304
 305                 if (d > distance) {
 306                         distance = d;
 307                         edge = i;
 308                         n = &polygon->normals[i];
 309                 }
 310         }
 311
 312         object->position.x -= (1 + elasticity) * distance * n->x;
 313         object->position.y -= (1 + elasticity) * distance * n->y;
 314
 315         distance =
 316                 n->x * (object->previous_position.x - polygon->points[edge].x) +
 317                 n->y * (object->previous_position.y - polygon->points[edge].y);
 318
 319         object->previous_position.x -= (1 + elasticity) * distance * n->x;
 320         object->previous_position.y -= (1 + elasticity) * distance * n->y;
 321 }
 322
 323 static void
 324 model_constrain_polygon (Model *model, Polygon *polygon)
 325 {
 326         int i;
 327
 328         for (i = 0; i < model->num_objects; i++) {
 329                 if (polygon_contains_point (polygon, &model->objects[i].position))
 330                         polygon_reflect_object (polygon, &model->objects[i]);
 331         }
 332 }
 333
 334 static void
 335 model_constrain_anchor (Model *model, Anchor *anchor)
 336 {
 337         anchor->object->position.x = anchor->x;
 338         anchor->object->position.y = anchor->y;
 339         anchor->object->previous_position.x = anchor->x;
 340         anchor->object->previous_position.y = anchor->y;
 341 }
 342
 343 static void
 344 model_constrain_offset (Model *model, Offset *offset)
 345 {
 346         double x, y;
 347         int i;
 348
 349         x = 0;
 350         y = 0;
 351         for (i = 0; i < offset->num_objects; i++) {
 352                 x += offset->objects[i]->position.x;
 353                 y += offset->objects[i]->position.y;
 354         }
 355
 356         x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
 357         y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;
 358
 359         for (i = 0; i < offset->num_objects; i++) {
 360                 offset->objects[i]->position.x = x + offset->dx * i;
 361                 offset->objects[i]->position.y = y + offset->dy * i;
 362         }
 363 }
 364
 365 static void
 366 model_constrain (Model *model)
 367 {
 368         double dx, dy, x, y, distance, fraction;
 369         int i;
 370
 371         if (model->mouse_anchor.object != NULL)
 372                 model_constrain_anchor (model, &model->mouse_anchor);
 373         for (i = 0; i < model->num_anchors; i++)
 374                 model_constrain_anchor (model, &model->anchors[i]);
 375
 376         /* String constraints. */
 377         for (i = 0; i < model->num_strings; i++) {
 378                 x = model->strings[i].a->position.x;
 379                 y = model->strings[i].a->position.y;
 380                 dx = model->strings[i].b->position.x - x;
 381                 dy = model->strings[i].b->position.y - y;
 382                 distance = estimate_distance (dx, dy, model->strings[i].length);
 383                 if (distance < model->strings[i].length)
 384                         continue;
 385                 fraction = (distance - model->strings[i].length) / distance / 2;
 386                 model->strings[i].a->position.x = x + dx * fraction;
 387                 model->strings[i].a->position.y = y + dy * fraction;
 388                 model->strings[i].b->position.x = x + dx * (1 - fraction);
 389                 model->strings[i].b->position.y = y + dy * (1 - fraction);
 390         }
 391
 392         /* Spacer constraints. */
 393         for (i = 0; i < model->num_spacers; i++) {
 394                 x = model->spacers[i].a->position.x;
 395                 y = model->spacers[i].a->position.y;
 396                 dx = model->spacers[i].b->position.x - x;
 397                 dy = model->spacers[i].b->position.y - y;
 398                 distance = estimate_distance (dx, dy, model->spacers[i].length);
 399                 if (distance > model->spacers[i].length)
 400                         continue;
 401                 fraction = (distance - model->spacers[i].length) / distance / 2;
 402                 model->spacers[i].a->position.x = x + dx * fraction;
 403                 model->spacers[i].a->position.y = y + dy * fraction;
 404                 model->spacers[i].b->position.x = x + dx * (1 - fraction);
 405                 model->spacers[i].b->position.y = y + dy * (1 - fraction);
 406         }
 407
 408         /* Stick constraints. */
 409         for (i = 0; i < model->num_sticks; i++) {
 410                 x = model->sticks[i].a->position.x;
 411                 y = model->sticks[i].a->position.y;
 412                 dx = model->sticks[i].b->position.x - x;
 413                 dy = model->sticks[i].b->position.y - y;
 414                 distance = estimate_distance (dx, dy, model->sticks[i].length);
 415                 fraction = (distance - model->sticks[i].length) / distance / 2;
 416                 model->sticks[i].a->position.x = x + dx * fraction;
 417                 model->sticks[i].a->position.y = y + dy * fraction;
 418                 model->sticks[i].b->position.x = x + dx * (1 - fraction);
 419                 model->sticks[i].b->position.y = y + dy * (1 - fraction);
 420         }
 421
 422         /* Offset constraints. */
 423         for (i = 0; i < model->num_offsets; i++)
 424                 model_constrain_offset (model, &model->offsets[i]);
 425
 426         /* Polygon constraints. */
 427         for (i = 0; i < model->num_polygons; i++)
 428                 model_constrain_polygon (model, &model->polygons[i]);
 429 }
 430
 431 void
 432 model_step (Model *model, double delta_t)
 433 {
 434         int i;
 435
 436         model_accumulate_forces (model);
 437         model_integrate (model, delta_t);
 438         for (i = 0; i < 20; i++)
 439                 model_constrain (model);
 440
 441         model->theta += delta_t;
 442 }
 443
 444 static double
 445 object_distance (Object *object, double x, double y)
 446 {
 447         double dx, dy;
 448
 449         dx = object->position.x - x;
 450         dy = object->position.y - y;
 451
 452         return sqrt (dx*dx + dy*dy);
 453 }
 454
 455 Object *
 456 model_find_nearest (Model *model, double x, double y)
 457 {
 458         Object *object;
 459         double distance, min_distance;
 460         int i;
 461
 462         for (i = 0; i < model->num_objects; i++) {
 463                 distance = object_distance (&model->objects[i], x, y);
 464                 if (i == 0 || distance < min_distance) {
 465                         min_distance = distance;
 466                         object = &model->objects[i];
 467                 }
 468         }
 469
 470         return object;
 471 }