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