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