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