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

   1 /*                                           -*- mode: c; c-basic-offset: 2 -*-
   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 = 1;
  25 const double gravity = 20;
  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 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
 101   for (i = 0; i < num_points; i++) {
 102     p->points[i].x = va_arg (ap, double);
 103     p->points[i].y = va_arg (ap, double);
 104   }
 105   va_end (ap);
 106
 107   p->normals = g_new (Vector, p->num_points);
 108   /* Compute outward pointing normals.  p->normals[i] is the normal
 109    * for the edged between p->points[i] and p->points[i + 1]. */
 110   for (i = 0; i < p->num_points; i++) {
 111     j = (i + 1) % p->num_points;
 112     dx = p->points[j].x - p->points[i].x;
 113     dy = p->points[j].y - p->points[i].y;
 114     length = sqrt (dx * dx + dy * dy);
 115     p->normals[i].x = -dy / length;
 116     p->normals[i].y = dx / length;
 117   }
 118 }
 119
 120 void
 121 polygon_init_diamond (Polygon *polygon, double x, double y)
 122 {
 123   return polygon_init (polygon, 5,
 124                        x, y,
 125                        x + 10, y + 40,
 126                        x + 90, y + 40,
 127                        x + 100, y,
 128                        x + 50, y - 20);
 129 }
 130
 131 void
 132 polygon_init_rectangle (Polygon *polygon, double x0, double y0,
 133                         double x1, double y1)
 134 {
 135   return polygon_init (polygon, 4, x0, y0, x0, y1, x1, y1, x1, y0);
 136 }
 137
 138 void
 139 model_fini (Model *model)
 140 {
 141   int i;
 142
 143   g_free (model->objects);
 144   g_free (model->sticks);
 145   g_free (model->strings);
 146   for (i = 0; i < model->num_offsets; i++)
 147     g_free (model->offsets[i].objects);
 148   g_free (model->springs);
 149   g_free (model->offset_springs);
 150   g_free (model->spacers);
 151   for (i = 0; i < model->num_polygons; i++)
 152     g_free (model->polygons[i].points);
 153   g_free (model->polygons);
 154
 155   memset (model, 0, sizeof *model);
 156 }
 157
 158 static void
 159 model_accumulate_forces (Model *model)
 160 {
 161   int i;
 162   double x, y, dx, dy, distance, displacement;
 163   Point middle;
 164   Vector u, v;
 165
 166   for (i = 0; i < model->num_objects; i++) {
 167     /* Gravity */
 168     model->objects[i].force.x = 0;
 169     model->objects[i].force.y = gravity * model->objects[i].mass;
 170
 171     /* Friction */
 172     v.x = model->objects[i].position.x - model->objects[i].previous_position.x;
 173     v.y = model->objects[i].position.y - model->objects[i].previous_position.y;
 174     model->objects[i].force.x -= v.x * friction;
 175     model->objects[i].force.y -= v.y * friction;
 176   }
 177
 178   for (i = 0; i < model->num_springs; i++) {
 179     x = model->springs[i].a->position.x;
 180     y = model->springs[i].a->position.y;
 181     dx = model->springs[i].b->position.x - x;
 182     dy = model->springs[i].b->position.y - y;
 183     distance = sqrt (dx * dx + dy * dy);
 184     u.x = dx / distance;
 185     u.y = dy / distance;
 186     displacement = distance - model->springs[i].length;
 187     model->springs[i].a->force.x += u.x * model->k * displacement;
 188     model->springs[i].a->force.y += u.y * model->k * displacement;
 189     model->springs[i].b->force.x -= u.x * model->k * displacement;
 190     model->springs[i].b->force.y -= u.y * model->k * displacement;
 191   }
 192
 193   for (i = 0; i < model->num_offset_springs; i++) {
 194     middle.x =
 195       (model->offset_springs[i].a->position.x +
 196        model->offset_springs[i].b->position.x) / 2;
 197     middle.y =
 198       (model->offset_springs[i].a->position.y +
 199        model->offset_springs[i].b->position.y) / 2;
 200
 201     x = middle.x - model->offset_springs[i].dx / 2;
 202     y = middle.y - model->offset_springs[i].dy / 2;
 203
 204     dx = x - model->offset_springs[i].a->position.x;
 205     dy = y - model->offset_springs[i].a->position.y;
 206
 207     model->offset_springs[i].a->force.x += dx * model->k;
 208     model->offset_springs[i].a->force.y += dy * model->k;
 209     model->offset_springs[i].b->force.x -= dx * model->k;
 210     model->offset_springs[i].b->force.y -= dy * model->k;
 211   }
 212
 213   for (i = 0; i < model->num_objects; i++) {
 214     double f =
 215       model->objects[i].force.x * model->objects[i].force.x +
 216       model->objects[i].force.y * model->objects[i].force.y;
 217
 218     if (f > 100000000)
 219       abort();
 220   }
 221 }
 222
 223 static void
 224 model_integrate (Model *model, double step)
 225 {
 226   double x, y;
 227   Object *o;
 228   int i;
 229
 230   for (i = 0; i < model->num_objects; i++) {
 231     o = &model->objects[i];
 232     x = o->position.x;
 233     y = o->position.y;
 234
 235     o->position.x =
 236       x + (x - o->previous_position.x) + o->force.x * step * step;
 237     o->position.y =
 238       y + (y - o->previous_position.y) + o->force.y * step * step;
 239
 240     o->previous_position.x = x;
 241     o->previous_position.y = y;
 242   }
 243 }
 244
 245 /* The square root in the distance computation for the string and
 246  * stick constraints can be aproximated using Newton:
 247  *
 248  *    distance =
 249  *      (model->sticks[i].length +
 250  *       (dx * dx + dy * dy) / model->sticks[i].length) / 2;
 251  *
 252  * This works really well, since the constraints aren't typically
 253  * violated much.  Thus, the distance is really close to the stick
 254  * length, which then makes a good initial guess.  However, the
 255  * approximation seems to be slower that just calling sqrt()...
 256  */
 257
 258 static inline double
 259 estimate_distance (double dx, double dy, double r)
 260 {
 261 #ifdef APPROXIMATE_SQUARE_ROOTS
 262   return (r + (dx * dx + dy * dy) / r) / 2;
 263 #else
 264   return sqrt (dx * dx + dy * dy);
 265 #endif
 266 }
 267
 268 static int
 269 polygon_contains_point (Polygon *polygon, Point *point)
 270 {
 271   int i;
 272   double dx, dy;
 273
 274   for (i = 0; i < polygon->num_points; i++) {
 275     dx = point->x - polygon->points[i].x;
 276     dy = point->y - polygon->points[i].y;
 277
 278     if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
 279       return FALSE;
 280   }
 281
 282   return TRUE;
 283 }
 284
 285 static void
 286 polygon_reflect_object (Polygon *polygon, Object *object)
 287 {
 288   int i, edge;
 289   double d, distance;
 290   Vector *n;
 291
 292   distance = -1000;
 293   for (i = 0; i < polygon->num_points; i++) {
 294     d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
 295       polygon->normals[i].y * (object->position.y - polygon->points[i].y);
 296
 297     if (d > distance) {
 298       distance = d;
 299       edge = i;
 300       polygon->edge = i;
 301       n = &polygon->normals[i];
 302     }
 303   }
 304
 305   object->position.x -= (1 + elasticity) * distance * n->x;
 306   object->position.y -= (1 + elasticity) * distance * n->y;
 307
 308   distance =
 309     n->x * (object->previous_position.x - polygon->points[edge].x) +
 310     n->y * (object->previous_position.y - polygon->points[edge].y);
 311
 312   object->previous_position.x -= (1 + elasticity) * distance * n->x;
 313   object->previous_position.y -= (1 + elasticity) * distance * n->y;
 314 }
 315
 316 static void
 317 model_constrain_polygon (Model *model, Polygon *polygon)
 318 {
 319   int i;
 320
 321   for (i = 0; i < model->num_objects; i++) {
 322     if (polygon_contains_point (polygon, &model->objects[i].position))
 323       polygon_reflect_object (polygon, &model->objects[i]);
 324   }
 325 }
 326
 327 static void
 328 model_constrain_anchor (Model *model, Anchor *anchor)
 329 {
 330   anchor->object->position.x = anchor->x;
 331   anchor->object->position.y = anchor->y;
 332   anchor->object->previous_position.x = anchor->x;
 333   anchor->object->previous_position.y = anchor->y;
 334 }
 335
 336 static void
 337 model_constrain_offset (Model *model, Offset *offset)
 338 {
 339   double x, y;
 340   int i;
 341
 342   x = 0;
 343   y = 0;
 344   for (i = 0; i < offset->num_objects; i++) {
 345     x += offset->objects[i]->position.x;
 346     y += offset->objects[i]->position.y;
 347   }
 348
 349   x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
 350   y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;
 351
 352   for (i = 0; i < offset->num_objects; i++) {
 353     offset->objects[i]->position.x = x + offset->dx * i;
 354     offset->objects[i]->position.y = y + offset->dy * i;
 355   }
 356 }
 357
 358 static void
 359 model_constrain (Model *model)
 360 {
 361   double dx, dy, x, y, distance, fraction;
 362   int i;
 363
 364   if (model->mouse_anchor.object != NULL)
 365     model_constrain_anchor (model, &model->mouse_anchor);
 366   for (i = 0; i < model->num_anchors; i++)
 367     model_constrain_anchor (model, &model->anchors[i]);
 368
 369   /* String constraints. */
 370   for (i = 0; i < model->num_strings; i++) {
 371     x = model->strings[i].a->position.x;
 372     y = model->strings[i].a->position.y;
 373     dx = model->strings[i].b->position.x - x;
 374     dy = model->strings[i].b->position.y - y;
 375     distance = estimate_distance (dx, dy, model->strings[i].length);
 376     if (distance < model->strings[i].length)
 377       continue;
 378     fraction = (distance - model->strings[i].length) / distance / 2;
 379     model->strings[i].a->position.x = x + dx * fraction;
 380     model->strings[i].a->position.y = y + dy * fraction;
 381     model->strings[i].b->position.x = x + dx * (1 - fraction);
 382     model->strings[i].b->position.y = y + dy * (1 - fraction);
 383   }
 384
 385   /* Spacer constraints. */
 386   for (i = 0; i < model->num_spacers; i++) {
 387     x = model->spacers[i].a->position.x;
 388     y = model->spacers[i].a->position.y;
 389     dx = model->spacers[i].b->position.x - x;
 390     dy = model->spacers[i].b->position.y - y;
 391     distance = estimate_distance (dx, dy, model->spacers[i].length);
 392     if (distance > model->spacers[i].length)
 393       continue;
 394     fraction = (distance - model->spacers[i].length) / distance / 2;
 395     model->spacers[i].a->position.x = x + dx * fraction;
 396     model->spacers[i].a->position.y = y + dy * fraction;
 397     model->spacers[i].b->position.x = x + dx * (1 - fraction);
 398     model->spacers[i].b->position.y = y + dy * (1 - fraction);
 399   }
 400
 401   /* Stick constraints. */
 402   for (i = 0; i < model->num_sticks; i++) {
 403     x = model->sticks[i].a->position.x;
 404     y = model->sticks[i].a->position.y;
 405     dx = model->sticks[i].b->position.x - x;
 406     dy = model->sticks[i].b->position.y - y;
 407     distance = estimate_distance (dx, dy, model->sticks[i].length);
 408     fraction = (distance - model->sticks[i].length) / distance / 2;
 409     model->sticks[i].a->position.x = x + dx * fraction;
 410     model->sticks[i].a->position.y = y + dy * fraction;
 411     model->sticks[i].b->position.x = x + dx * (1 - fraction);
 412     model->sticks[i].b->position.y = y + dy * (1 - fraction);
 413   }
 414
 415   /* Offset constraints. */
 416   for (i = 0; i < model->num_offsets; i++)
 417     model_constrain_offset (model, &model->offsets[i]);
 418
 419   /* Polygon constraints. */
 420   for (i = 0; i < model->num_polygons; i++)
 421     model_constrain_polygon (model, &model->polygons[i]);
 422 }
 423
 424 void
 425 model_step (Model *model, double delta_t)
 426 {
 427   int i;
 428
 429   model_accumulate_forces (model);
 430   model_integrate (model, delta_t);
 431   for (i = 0; i < 2; i++)
 432     model_constrain (model);
 433
 434   model->theta += delta_t;
 435 }
 436
 437 static double
 438 object_distance (Object *object, double x, double y)
 439 {
 440   double dx, dy;
 441
 442   dx = object->position.x - x;
 443   dy = object->position.y - y;
 444
 445   return sqrt (dx*dx + dy*dy);
 446 }
 447
 448 Object *
 449 model_find_nearest (Model *model, double x, double y)
 450 {
 451   Object *object;
 452   double distance, min_distance;
 453   int i;
 454
 455   for (i = 0; i < model->num_objects; i++) {
 456     distance = object_distance (&model->objects[i], x, y);
 457     if (i == 0 || distance < min_distance) {
 458       min_distance = distance;
 459       object = &model->objects[i];
 460     }
 461   }
 462
 463   return object;
 464 }