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