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