1 /* -*- mode: c; c-basic-offset: 2 -*-
4 * http://en.wikipedia.org/wiki/Verlet_integration
5 * http://www.teknikus.dk/tj/gdc2001.htm
9 * - Add code to add boxes
11 * - Try out this idea: make constraint solver take mean of all
12 * corrections at the end instead of meaning as it goes.
23 const double elasticity = 0.7;
24 const double friction = 8;
25 const double gravity = 50;
28 object_init (Object *object, double x, double y, double mass)
30 object->position.x = x;
31 object->position.y = y;
32 object->previous_position.x = x;
33 object->previous_position.y = y;
38 spring_init (Spring *spring, Object *a, Object *b, double length)
42 spring->length = length;
46 stick_init (Stick *stick, Object *a, Object *b, double length)
50 stick->length = length;
54 string_init (String *string, Object *a, Object *b, double length)
58 string->length = length;
62 offset_spring_init (OffsetSpring *spring, Object *a, Object *b,
72 spacer_init (Spacer *spacer, Object *a, Object *b, double length)
76 spacer->length = length;
80 anchor_init (Anchor *anchor, Object *object, double x, double y)
82 anchor->object = object;
88 polygon_init (Polygon *p, int enclosing, int num_points, ...)
90 double dx, dy, length;
94 /* Polygons are defined counter-clock-wise in a coordinate system
95 * with the y-axis pointing down. */
97 va_start (ap, num_points);
98 p->num_points = num_points;
99 p->points = g_new (Point, num_points);
100 p->enclosing = enclosing;
102 for (i = 0; i < num_points; i++) {
103 p->points[i].x = va_arg (ap, double);
104 p->points[i].y = va_arg (ap, double);
108 p->normals = g_new (Vector, p->num_points);
109 /* Compute outward pointing normals. p->normals[i] is the normal
110 * for the edged between p->points[i] and p->points[i + 1]. */
111 for (i = 0; i < p->num_points; i++) {
112 j = (i + 1) % p->num_points;
113 dx = p->points[j].x - p->points[i].x;
114 dy = p->points[j].y - p->points[i].y;
115 length = sqrt (dx * dx + dy * dy);
116 p->normals[i].x = -dy / length;
117 p->normals[i].y = dx / length;
122 polygon_init_diamond (Polygon *polygon, double x, double y)
124 return polygon_init (polygon, FALSE, 5,
133 polygon_init_rectangle (Polygon *polygon, double x0, double y0,
134 double x1, double y1)
136 return polygon_init (polygon, FALSE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
140 polygon_init_enclosing_rectangle (Polygon *polygon, double x0, double y0,
141 double x1, double y1)
143 return polygon_init (polygon, TRUE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
147 model_fini (Model *model)
151 g_free (model->objects);
152 g_free (model->sticks);
153 g_free (model->strings);
154 for (i = 0; i < model->num_offsets; i++)
155 g_free (model->offsets[i].objects);
156 g_free (model->springs);
157 g_free (model->offset_springs);
158 g_free (model->spacers);
159 for (i = 0; i < model->num_polygons; i++)
160 g_free (model->polygons[i].points);
161 g_free (model->polygons);
163 memset (model, 0, sizeof *model);
167 model_accumulate_forces (Model *model)
170 double x, y, dx, dy, distance, displacement;
174 for (i = 0; i < model->num_objects; i++) {
176 model->objects[i].force.x = 0;
177 model->objects[i].force.y = gravity * model->objects[i].mass;
180 v.x = model->objects[i].position.x - model->objects[i].previous_position.x;
181 v.y = model->objects[i].position.y - model->objects[i].previous_position.y;
182 model->objects[i].force.x -= v.x * friction;
183 model->objects[i].force.y -= v.y * friction;
186 for (i = 0; i < model->num_springs; i++) {
187 x = model->springs[i].a->position.x;
188 y = model->springs[i].a->position.y;
189 dx = model->springs[i].b->position.x - x;
190 dy = model->springs[i].b->position.y - y;
191 distance = sqrt (dx * dx + dy * dy);
194 displacement = distance - model->springs[i].length;
195 model->springs[i].a->force.x += u.x * model->k * displacement;
196 model->springs[i].a->force.y += u.y * model->k * displacement;
197 model->springs[i].b->force.x -= u.x * model->k * displacement;
198 model->springs[i].b->force.y -= u.y * model->k * displacement;
201 for (i = 0; i < model->num_offset_springs; i++) {
203 (model->offset_springs[i].a->position.x +
204 model->offset_springs[i].b->position.x) / 2;
206 (model->offset_springs[i].a->position.y +
207 model->offset_springs[i].b->position.y) / 2;
209 x = middle.x - model->offset_springs[i].dx / 2;
210 y = middle.y - model->offset_springs[i].dy / 2;
212 dx = x - model->offset_springs[i].a->position.x;
213 dy = y - model->offset_springs[i].a->position.y;
215 model->offset_springs[i].a->force.x += dx * model->k;
216 model->offset_springs[i].a->force.y += dy * model->k;
217 model->offset_springs[i].b->force.x -= dx * model->k;
218 model->offset_springs[i].b->force.y -= dy * model->k;
221 for (i = 0; i < model->num_objects; i++) {
223 model->objects[i].force.x * model->objects[i].force.x +
224 model->objects[i].force.y * model->objects[i].force.y;
232 model_integrate (Model *model, double step)
238 for (i = 0; i < model->num_objects; i++) {
239 o = &model->objects[i];
244 x + (x - o->previous_position.x) + o->force.x * step * step;
246 y + (y - o->previous_position.y) + o->force.y * step * step;
248 o->previous_position.x = x;
249 o->previous_position.y = y;
253 /* The square root in the distance computation for the string and
254 * stick constraints can be aproximated using Newton:
257 * (model->sticks[i].length +
258 * (dx * dx + dy * dy) / model->sticks[i].length) / 2;
260 * This works really well, since the constraints aren't typically
261 * violated much. Thus, the distance is really close to the stick
262 * length, which then makes a good initial guess. However, the
263 * approximation seems to be slower that just calling sqrt()...
267 estimate_distance (double dx, double dy, double r)
269 #ifdef APPROXIMATE_SQUARE_ROOTS
270 return (r + (dx * dx + dy * dy) / r) / 2;
272 return sqrt (dx * dx + dy * dy);
277 polygon_contains_point (Polygon *polygon, Point *point)
282 for (i = 0; i < polygon->num_points; i++) {
283 dx = point->x - polygon->points[i].x;
284 dy = point->y - polygon->points[i].y;
286 if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
287 return polygon->enclosing;
290 return !polygon->enclosing;
294 polygon_reflect_object (Polygon *polygon, Object *object)
301 for (i = 0; i < polygon->num_points; i++) {
302 d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
303 polygon->normals[i].y * (object->position.y - polygon->points[i].y);
308 n = &polygon->normals[i];
312 object->position.x -= (1 + elasticity) * distance * n->x;
313 object->position.y -= (1 + elasticity) * distance * n->y;
316 n->x * (object->previous_position.x - polygon->points[edge].x) +
317 n->y * (object->previous_position.y - polygon->points[edge].y);
319 object->previous_position.x -= (1 + elasticity) * distance * n->x;
320 object->previous_position.y -= (1 + elasticity) * distance * n->y;
324 model_constrain_polygon (Model *model, Polygon *polygon)
328 for (i = 0; i < model->num_objects; i++) {
329 if (polygon_contains_point (polygon, &model->objects[i].position))
330 polygon_reflect_object (polygon, &model->objects[i]);
335 model_constrain_anchor (Model *model, Anchor *anchor)
337 anchor->object->position.x = anchor->x;
338 anchor->object->position.y = anchor->y;
339 anchor->object->previous_position.x = anchor->x;
340 anchor->object->previous_position.y = anchor->y;
344 model_constrain_offset (Model *model, Offset *offset)
351 for (i = 0; i < offset->num_objects; i++) {
352 x += offset->objects[i]->position.x;
353 y += offset->objects[i]->position.y;
356 x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
357 y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;
359 for (i = 0; i < offset->num_objects; i++) {
360 offset->objects[i]->position.x = x + offset->dx * i;
361 offset->objects[i]->position.y = y + offset->dy * i;
366 model_constrain (Model *model)
368 double dx, dy, x, y, distance, fraction;
371 if (model->mouse_anchor.object != NULL)
372 model_constrain_anchor (model, &model->mouse_anchor);
373 for (i = 0; i < model->num_anchors; i++)
374 model_constrain_anchor (model, &model->anchors[i]);
376 /* String constraints. */
377 for (i = 0; i < model->num_strings; i++) {
378 x = model->strings[i].a->position.x;
379 y = model->strings[i].a->position.y;
380 dx = model->strings[i].b->position.x - x;
381 dy = model->strings[i].b->position.y - y;
382 distance = estimate_distance (dx, dy, model->strings[i].length);
383 if (distance < model->strings[i].length)
385 fraction = (distance - model->strings[i].length) / distance / 2;
386 model->strings[i].a->position.x = x + dx * fraction;
387 model->strings[i].a->position.y = y + dy * fraction;
388 model->strings[i].b->position.x = x + dx * (1 - fraction);
389 model->strings[i].b->position.y = y + dy * (1 - fraction);
392 /* Spacer constraints. */
393 for (i = 0; i < model->num_spacers; i++) {
394 x = model->spacers[i].a->position.x;
395 y = model->spacers[i].a->position.y;
396 dx = model->spacers[i].b->position.x - x;
397 dy = model->spacers[i].b->position.y - y;
398 distance = estimate_distance (dx, dy, model->spacers[i].length);
399 if (distance > model->spacers[i].length)
401 fraction = (distance - model->spacers[i].length) / distance / 2;
402 model->spacers[i].a->position.x = x + dx * fraction;
403 model->spacers[i].a->position.y = y + dy * fraction;
404 model->spacers[i].b->position.x = x + dx * (1 - fraction);
405 model->spacers[i].b->position.y = y + dy * (1 - fraction);
408 /* Stick constraints. */
409 for (i = 0; i < model->num_sticks; i++) {
410 x = model->sticks[i].a->position.x;
411 y = model->sticks[i].a->position.y;
412 dx = model->sticks[i].b->position.x - x;
413 dy = model->sticks[i].b->position.y - y;
414 distance = estimate_distance (dx, dy, model->sticks[i].length);
415 fraction = (distance - model->sticks[i].length) / distance / 2;
416 model->sticks[i].a->position.x = x + dx * fraction;
417 model->sticks[i].a->position.y = y + dy * fraction;
418 model->sticks[i].b->position.x = x + dx * (1 - fraction);
419 model->sticks[i].b->position.y = y + dy * (1 - fraction);
422 /* Offset constraints. */
423 for (i = 0; i < model->num_offsets; i++)
424 model_constrain_offset (model, &model->offsets[i]);
426 /* Polygon constraints. */
427 for (i = 0; i < model->num_polygons; i++)
428 model_constrain_polygon (model, &model->polygons[i]);
432 model_step (Model *model, double delta_t)
436 model_accumulate_forces (model);
437 model_integrate (model, delta_t);
438 for (i = 0; i < 20; i++)
439 model_constrain (model);
441 model->theta += delta_t;
445 object_distance (Object *object, double x, double y)
449 dx = object->position.x - x;
450 dy = object->position.y - y;
452 return sqrt (dx*dx + dy*dy);
456 model_find_nearest (Model *model, double x, double y)
459 double distance, min_distance;
462 for (i = 0; i < model->num_objects; i++) {
463 distance = object_distance (&model->objects[i], x, y);
464 if (i == 0 || distance < min_distance) {
465 min_distance = distance;
466 object = &model->objects[i];