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