359c46f2b0e4102653e1da082d92cad707006935
[blender.git] / source / blender / blenkernel / intern / rigidbody.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2013 Blender Foundation
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): Joshua Leung, Sergej Reich
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file rigidbody.c
29  *  \ingroup blenkernel
30  *  \brief Blender-side interface and methods for dealing with Rigid Body simulations
31  */
32
33 #include <stdio.h>
34 #include <string.h>
35 #include <stddef.h>
36 #include <float.h>
37 #include <math.h>
38 #include <limits.h>
39
40 #include "MEM_guardedalloc.h"
41
42 #include "BLI_blenlib.h"
43 #include "BLI_math.h"
44
45 #include "RBI_api.h"
46
47 #include "DNA_anim_types.h"
48 #include "DNA_group_types.h"
49 #include "DNA_mesh_types.h"
50 #include "DNA_meshdata_types.h"
51 #include "DNA_object_types.h"
52 #include "DNA_object_force.h"
53 #include "DNA_rigidbody_types.h"
54 #include "DNA_scene_types.h"
55
56 #include "BKE_animsys.h"
57 #include "BKE_cdderivedmesh.h"
58 #include "BKE_effect.h"
59 #include "BKE_group.h"
60 #include "BKE_object.h"
61 #include "BKE_mesh.h"
62 #include "BKE_pointcache.h"
63 #include "BKE_rigidbody.h"
64 #include "BKE_global.h"
65 #include "BKE_utildefines.h"
66
67 #include "RNA_access.h"
68
69 /* ************************************** */
70 /* Memory Management */
71
72 /* Freeing Methods --------------------- */
73
74 /* Free rigidbody world */
75 void BKE_rigidbody_free_world(RigidBodyWorld *rbw)
76 {
77         GroupObject *go;
78         /* sanity check */
79         if (!rbw)
80                 return;
81
82         if (rbw->physics_world) {
83                 /* free physics references, we assume that all physics objects in will have been added to the world */
84                 if (rbw->group) {
85                         for (go = rbw->group->gobject.first; go; go = go->next) {
86                                 if (go->ob && go->ob->rigidbody_object) {
87                                         RigidBodyOb *rbo = go->ob->rigidbody_object;
88
89                                         if (rbo->physics_object)
90                                                 RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
91                                 }
92                         }
93                 }
94                 /* free dynamics world */
95                 RB_dworld_delete(rbw->physics_world);
96         }
97         if (rbw->objects)
98                 free(rbw->objects);
99
100         /* free cache */
101         BKE_ptcache_free_list(&(rbw->ptcaches));
102         rbw->pointcache = NULL;
103
104         /* free effector weights */
105         if (rbw->effector_weights)
106                 MEM_freeN(rbw->effector_weights);
107
108         /* free rigidbody world itself */
109         MEM_freeN(rbw);
110 }
111
112 /* Free RigidBody settings and sim instances */
113 void BKE_rigidbody_free_object(Object *ob)
114 {
115         RigidBodyOb *rbo = (ob) ? ob->rigidbody_object : NULL;
116
117         /* sanity check */
118         if (rbo == NULL)
119                 return;
120
121         /* free physics references */
122         if (rbo->physics_object) {
123                 RB_body_delete(rbo->physics_object);
124                 rbo->physics_object = NULL;
125         }
126
127         if (rbo->physics_shape) {
128                 RB_shape_delete(rbo->physics_shape);
129                 rbo->physics_shape = NULL;
130         }
131
132         /* free data itself */
133         MEM_freeN(rbo);
134         ob->rigidbody_object = NULL;
135 }
136
137 /* Copying Methods --------------------- */
138
139 /* These just copy the data, clearing out references to physics objects.
140  * Anything that uses them MUST verify that the copied object will
141  * be added to relevant groups later...
142  */
143
144 RigidBodyOb *BKE_rigidbody_copy_object(Object *ob)
145 {
146         RigidBodyOb *rboN = NULL;
147
148         if (ob->rigidbody_object) {
149                 /* just duplicate the whole struct first (to catch all the settings) */
150                 rboN = MEM_dupallocN(ob->rigidbody_object);
151
152                 /* tag object as needing to be verified */
153                 rboN->flag |= RBO_FLAG_NEEDS_VALIDATE;
154
155                 /* clear out all the fields which need to be revalidated later */
156                 rboN->physics_object = NULL;
157                 rboN->physics_shape = NULL;
158         }
159
160         /* return new copy of settings */
161         return rboN;
162 }
163
164 /* ************************************** */
165 /* Setup Utilities - Validate Sim Instances */
166
167 /* create collision shape of mesh - convex hull */
168 static rbCollisionShape *rigidbody_get_shape_convexhull_from_mesh(Object *ob, float margin, bool *can_embed)
169 {
170         rbCollisionShape *shape = NULL;
171         Mesh *me = NULL;
172
173         if (ob->type == OB_MESH && ob->data) {
174                 me = ob->data;
175         }
176         else {
177                 printf("ERROR: cannot make Convex Hull collision shape for non-Mesh object\n");
178         }
179
180         if (me && me->totvert) {
181                 shape = RB_shape_new_convex_hull((float *)me->mvert, sizeof(MVert), me->totvert, margin, can_embed);
182         }
183         else {
184                 printf("ERROR: no vertices to define Convex Hull collision shape with\n");
185         }
186
187         return shape;
188 }
189
190 /* create collision shape of mesh - triangulated mesh
191  * returns NULL if creation fails.
192  */
193 static rbCollisionShape *rigidbody_get_shape_trimesh_from_mesh(Object *ob)
194 {
195         rbCollisionShape *shape = NULL;
196
197         if (ob->type == OB_MESH) {
198                 DerivedMesh *dm = CDDM_from_mesh(ob->data, ob);
199
200                 MVert *mvert;
201                 MFace *mface;
202                 int totvert;
203                 int totface;
204
205                 /* ensure mesh validity, then grab data */
206                 DM_ensure_tessface(dm);
207
208                 mvert   = (dm) ? dm->getVertArray(dm) : NULL;
209                 totvert = (dm) ? dm->getNumVerts(dm) : 0;
210                 mface   = (dm) ? dm->getTessFaceArray(dm) : NULL;
211                 totface = (dm) ? dm->getNumTessFaces(dm) : 0;
212
213                 /* sanity checking - potential case when no data will be present */
214                 if ((totvert == 0) || (totface == 0)) {
215                         printf("WARNING: no geometry data converted for Mesh Collision Shape (ob = %s)\n", ob->id.name + 2);
216                 }
217                 else {
218                         rbMeshData *mdata;
219                         int i;
220
221                         /* init mesh data for collision shape */
222                         mdata = RB_trimesh_data_new();
223
224                         /* loop over all faces, adding them as triangles to the collision shape
225                          * (so for some faces, more than triangle will get added)
226                          */
227                         for (i = 0; (i < totface) && (mface) && (mvert); i++, mface++) {
228                                 /* add first triangle - verts 1,2,3 */
229                                 {
230                                         MVert *va = (IN_RANGE(mface->v1, 0, totvert)) ? (mvert + mface->v1) : (mvert);
231                                         MVert *vb = (IN_RANGE(mface->v2, 0, totvert)) ? (mvert + mface->v2) : (mvert);
232                                         MVert *vc = (IN_RANGE(mface->v3, 0, totvert)) ? (mvert + mface->v3) : (mvert);
233
234                                         RB_trimesh_add_triangle(mdata, va->co, vb->co, vc->co);
235                                 }
236
237                                 /* add second triangle if needed - verts 1,3,4 */
238                                 if (mface->v4) {
239                                         MVert *va = (IN_RANGE(mface->v1, 0, totvert)) ? (mvert + mface->v1) : (mvert);
240                                         MVert *vb = (IN_RANGE(mface->v3, 0, totvert)) ? (mvert + mface->v3) : (mvert);
241                                         MVert *vc = (IN_RANGE(mface->v4, 0, totvert)) ? (mvert + mface->v4) : (mvert);
242
243                                         RB_trimesh_add_triangle(mdata, va->co, vb->co, vc->co);
244                                 }
245                         }
246
247                         /* construct collision shape
248                          *
249                          * These have been chosen to get better speed/accuracy tradeoffs with regards
250                          * to limitations of each:
251                          *    - BVH-Triangle Mesh: for passive objects only. Despite having greater
252                          *                         speed/accuracy, they cannot be used for moving objects.
253                          *    - GImpact Mesh:      for active objects. These are slower and less stable,
254                          *                         but are more flexible for general usage.
255                          */
256                         if (ob->rigidbody_object->type == RBO_TYPE_PASSIVE) {
257                                 shape = RB_shape_new_trimesh(mdata);
258                         }
259                         else {
260                                 shape = RB_shape_new_gimpact_mesh(mdata);
261                         }
262                 }
263
264                 /* cleanup temp data */
265                 if (dm) {
266                         dm->release(dm);
267                 }
268         }
269         else {
270                 printf("ERROR: cannot make Triangular Mesh collision shape for non-Mesh object\n");
271         }
272
273         return shape;
274 }
275
276 /* Create new physics sim collision shape for object and store it,
277  * or remove the existing one first and replace...
278  */
279 void BKE_rigidbody_validate_sim_shape(Object *ob, short rebuild)
280 {
281         RigidBodyOb *rbo = ob->rigidbody_object;
282         rbCollisionShape *new_shape = NULL;
283         BoundBox *bb = NULL;
284         float size[3] = {1.0f, 1.0f, 1.0f};
285         float radius = 1.0f;
286         float height = 1.0f;
287         float capsule_height;
288         float hull_margin = 0.0f;
289         bool can_embed = true;
290
291         /* sanity check */
292         if (rbo == NULL)
293                 return;
294
295         /* don't create a new shape if we already have one and don't want to rebuild it */
296         if (rbo->physics_shape && !rebuild)
297                 return;
298
299         /* if automatically determining dimensions, use the Object's boundbox
300          *      - assume that all quadrics are standing upright on local z-axis
301          *      - assume even distribution of mass around the Object's pivot
302          *        (i.e. Object pivot is centralised in boundbox)
303          */
304         // XXX: all dimensions are auto-determined now... later can add stored settings for this
305         /* get object dimensions without scaling */
306         bb = BKE_object_boundbox_get(ob);
307         if (bb) {
308                 size[0] = (bb->vec[4][0] - bb->vec[0][0]);
309                 size[1] = (bb->vec[2][1] - bb->vec[0][1]);
310                 size[2] = (bb->vec[1][2] - bb->vec[0][2]);
311         }
312         mul_v3_fl(size, 0.5f);
313
314         if (ELEM3(rbo->shape, RB_SHAPE_CAPSULE, RB_SHAPE_CYLINDER, RB_SHAPE_CONE)) {
315                 /* take radius as largest x/y dimension, and height as z-dimension */
316                 radius = MAX2(size[0], size[1]);
317                 height = size[2];
318         }
319         else if (rbo->shape == RB_SHAPE_SPHERE) {
320                 /* take radius to the the largest dimension to try and encompass everything */
321                 radius = MAX3(size[0], size[1], size[2]);
322         }
323
324         /* create new shape */
325         switch (rbo->shape) {
326                 case RB_SHAPE_BOX:
327                         new_shape = RB_shape_new_box(size[0], size[1], size[2]);
328                         break;
329
330                 case RB_SHAPE_SPHERE:
331                         new_shape = RB_shape_new_sphere(radius);
332                         break;
333
334                 case RB_SHAPE_CAPSULE:
335                         capsule_height = (height - radius) * 2.0f;
336                         new_shape = RB_shape_new_capsule(radius, (capsule_height > 0.0f) ? capsule_height : 0.0f);
337                         break;
338                 case RB_SHAPE_CYLINDER:
339                         new_shape = RB_shape_new_cylinder(radius, height);
340                         break;
341                 case RB_SHAPE_CONE:
342                         new_shape = RB_shape_new_cone(radius, height * 2.0f);
343                         break;
344
345                 case RB_SHAPE_CONVEXH:
346                         /* try to emged collision margin */
347                         if (!(rbo->flag & RBO_FLAG_USE_MARGIN))
348                                 hull_margin = 0.04f;
349                         new_shape = rigidbody_get_shape_convexhull_from_mesh(ob, hull_margin, &can_embed);
350                         if (!(rbo->flag & RBO_FLAG_USE_MARGIN))
351                                 rbo->margin = (can_embed) ? 0.04f : 0.0f;  /* RB_TODO ideally we shouldn't directly change the margin here */
352                         break;
353                 case RB_SHAPE_TRIMESH:
354                         new_shape = rigidbody_get_shape_trimesh_from_mesh(ob);
355                         break;
356         }
357         /* assign new collision shape if creation was successful */
358         if (new_shape) {
359                 if (rbo->physics_shape)
360                         RB_shape_delete(rbo->physics_shape);
361                 rbo->physics_shape = new_shape;
362                 RB_shape_set_margin(rbo->physics_shape, RBO_GET_MARGIN(rbo));
363         }
364 }
365
366 /* --------------------- */
367
368 /* Create physics sim representation of object given RigidBody settings
369  * < rebuild: even if an instance already exists, replace it
370  */
371 void BKE_rigidbody_validate_sim_object(RigidBodyWorld *rbw, Object *ob, short rebuild)
372 {
373         RigidBodyOb *rbo = (ob) ? ob->rigidbody_object : NULL;
374         float loc[3];
375         float rot[4];
376
377         /* sanity checks:
378          *      - object doesn't have RigidBody info already: then why is it here?
379          */
380         if (rbo == NULL)
381                 return;
382
383         /* make sure collision shape exists */
384         if (rbo->physics_shape == NULL || rebuild)
385                 BKE_rigidbody_validate_sim_shape(ob, true);
386
387         if (rbo->physics_object) {
388                 if (rebuild == false)
389                         RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
390         }
391         if (!rbo->physics_object || rebuild) {
392                 /* remove rigid body if it already exists before creating a new one */
393                 if (rbo->physics_object) {
394                         RB_body_delete(rbo->physics_object);
395                 }
396
397                 mat4_to_loc_quat(loc, rot, ob->obmat);
398
399                 rbo->physics_object = RB_body_new(rbo->physics_shape, loc, rot);
400
401                 RB_body_set_friction(rbo->physics_object, rbo->friction);
402                 RB_body_set_restitution(rbo->physics_object, rbo->restitution);
403
404                 RB_body_set_damping(rbo->physics_object, rbo->lin_damping, rbo->ang_damping);
405                 RB_body_set_sleep_thresh(rbo->physics_object, rbo->lin_sleep_thresh, rbo->ang_sleep_thresh);
406                 RB_body_set_activation_state(rbo->physics_object, rbo->flag & RBO_FLAG_USE_DEACTIVATION);
407
408                 if (rbo->type == RBO_TYPE_PASSIVE || rbo->flag & RBO_FLAG_START_DEACTIVATED)
409                         RB_body_deactivate(rbo->physics_object);
410
411
412                 RB_body_set_linear_factor(rbo->physics_object,
413                                                                   (ob->protectflag & OB_LOCK_LOCX) == 0,
414                                           (ob->protectflag & OB_LOCK_LOCY) == 0,
415                                           (ob->protectflag & OB_LOCK_LOCZ) == 0);
416                 RB_body_set_angular_factor(rbo->physics_object,
417                                            (ob->protectflag & OB_LOCK_ROTX) == 0,
418                                            (ob->protectflag & OB_LOCK_ROTY) == 0,
419                                            (ob->protectflag & OB_LOCK_ROTZ) == 0);
420
421                 RB_body_set_mass(rbo->physics_object, RBO_GET_MASS(rbo));
422                 RB_body_set_kinematic_state(rbo->physics_object, rbo->flag & RBO_FLAG_KINEMATIC || rbo->flag & RBO_FLAG_DISABLED);
423         }
424
425         if (rbw && rbw->physics_world)
426                 RB_dworld_add_body(rbw->physics_world, rbo->physics_object, rbo->col_groups);
427 }
428
429 /* --------------------- */
430
431 /* Create physics sim world given RigidBody world settings */
432 // NOTE: this does NOT update object references that the scene uses, in case those aren't ready yet!
433 void BKE_rigidbody_validate_sim_world(Scene *scene, RigidBodyWorld *rbw, short rebuild)
434 {
435         /* sanity checks */
436         if (rbw == NULL)
437                 return;
438
439         /* create new sim world */
440         if (rebuild || rbw->physics_world == NULL) {
441                 if (rbw->physics_world)
442                         RB_dworld_delete(rbw->physics_world);
443                 rbw->physics_world = RB_dworld_new(scene->physics_settings.gravity);
444         }
445
446         RB_dworld_set_solver_iterations(rbw->physics_world, rbw->num_solver_iterations);
447         RB_dworld_set_split_impulse(rbw->physics_world, rbw->flag & RBW_FLAG_USE_SPLIT_IMPULSE);
448 }
449
450 /* ************************************** */
451 /* Setup Utilities - Create Settings Blocks */
452
453 /* Set up RigidBody world */
454 RigidBodyWorld *BKE_rigidbody_create_world(Scene *scene)
455 {
456         /* try to get whatever RigidBody world that might be representing this already */
457         RigidBodyWorld *rbw;
458
459         /* sanity checks
460          *      - there must be a valid scene to add world to
461          *      - there mustn't be a sim world using this group already
462          */
463         if (scene == NULL)
464                 return NULL;
465
466         /* create a new sim world */
467         rbw = MEM_callocN(sizeof(RigidBodyWorld), "RigidBodyWorld");
468
469         /* set default settings */
470         rbw->effector_weights = BKE_add_effector_weights(NULL);
471
472         rbw->ltime = PSFRA;
473
474         rbw->time_scale = 1.0f;
475
476         rbw->steps_per_second = 60; /* Bullet default (60 Hz) */
477         rbw->num_solver_iterations = 10; /* 10 is bullet default */
478
479         rbw->pointcache = BKE_ptcache_add(&(rbw->ptcaches));
480         rbw->pointcache->step = 1;
481
482         /* return this sim world */
483         return rbw;
484 }
485
486 /* Add rigid body settings to the specified object */
487 RigidBodyOb *BKE_rigidbody_create_object(Scene *scene, Object *ob, short type)
488 {
489         RigidBodyOb *rbo;
490         RigidBodyWorld *rbw = scene->rigidbody_world;
491
492         /* sanity checks
493          *      - rigidbody world must exist
494          *      - object must exist
495          *      - cannot add rigid body if it already exists
496          */
497         if (ob == NULL || (ob->rigidbody_object != NULL))
498                 return NULL;
499
500         /* create new settings data, and link it up */
501         rbo = MEM_callocN(sizeof(RigidBodyOb), "RigidBodyOb");
502
503         /* set default settings */
504         rbo->type = type;
505
506         rbo->mass = 1.0f;
507
508         rbo->friction = 0.5f; /* best when non-zero. 0.5 is Bullet default */
509         rbo->restitution = 0.0f; /* best when zero. 0.0 is Bullet default */
510
511         rbo->margin = 0.04f; /* 0.04 (in meters) is Bullet default */
512
513         rbo->lin_sleep_thresh = 0.4f; /* 0.4 is half of Bullet default */
514         rbo->ang_sleep_thresh = 0.5f; /* 0.5 is half of Bullet default */
515
516         rbo->lin_damping = 0.04f; /* 0.04 is game engine default */
517         rbo->ang_damping = 0.1f; /* 0.1 is game engine default */
518
519         rbo->col_groups = 1;
520
521         /* use triangle meshes for passive objects
522          * use convex hulls for active objects since dynamic triangle meshes are very unstable
523          */
524         if (type == RBO_TYPE_ACTIVE)
525                 rbo->shape = RB_SHAPE_CONVEXH;
526         else
527                 rbo->shape = RB_SHAPE_TRIMESH;
528
529         /* set initial transform */
530         mat4_to_loc_quat(rbo->pos, rbo->orn, ob->obmat);
531
532         /* flag cache as outdated */
533         BKE_rigidbody_cache_reset(rbw);
534
535         /* return this object */
536         return rbo;
537 }
538
539 /* ************************************** */
540 /* Utilities API */
541
542 /* Get RigidBody world for the given scene, creating one if needed
543  * < scene: Scene to find active Rigid Body world for
544  */
545 RigidBodyWorld *BKE_rigidbody_get_world(Scene *scene)
546 {
547         /* sanity check */
548         if (scene == NULL)
549                 return NULL;
550
551         return scene->rigidbody_world;
552 }
553
554 void BKE_rigidbody_remove_object(Scene *scene, Object *ob)
555 {
556         RigidBodyWorld *rbw = scene->rigidbody_world;
557         RigidBodyOb *rbo = ob->rigidbody_object;
558         GroupObject *go;
559         int i;
560
561         if (rbw) {
562                 /* remove from rigidbody world, free object won't do this */
563                 if (rbw->physics_world && rbo->physics_object)
564                         RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
565
566                 /* remove object from array */
567                 if (rbw && rbw->objects) {
568                         for (i = 0; i < rbw->numbodies; i++) {
569                                 if (rbw->objects[i] == ob) {
570                                         rbw->objects[i] = NULL;
571                                         break;
572                                 }
573                         }
574                 }
575         }
576         /* remove object's settings */
577         BKE_rigidbody_free_object(ob);
578         /* flag cache as outdated */
579         BKE_rigidbody_cache_reset(rbw);
580 }
581
582
583 /* ************************************** */
584 /* Simulation Interface - Bullet */
585
586 /* Update object array and rigid body count so they're in sync with the rigid body group */
587 static void rigidbody_update_ob_array(RigidBodyWorld *rbw)
588 {
589         GroupObject *go;
590         int i, n;
591
592         n = BLI_countlist(&rbw->group->gobject);
593
594         if (rbw->numbodies != n) {
595                 rbw->numbodies = n;
596                 rbw->objects = realloc(rbw->objects, sizeof(Object *) * rbw->numbodies);
597         }
598
599         for (go = rbw->group->gobject.first, i = 0; go; go = go->next, i++) {
600                 Object *ob = go->ob;
601                 rbw->objects[i] = ob;
602         }
603 }
604
605 static void rigidbody_update_sim_world(Scene *scene, RigidBodyWorld *rbw)
606 {
607         float adj_gravity[3];
608
609         /* adjust gravity to take effector weights into account */
610         if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
611                 copy_v3_v3(adj_gravity, scene->physics_settings.gravity);
612                 mul_v3_fl(adj_gravity, rbw->effector_weights->global_gravity * rbw->effector_weights->weight[0]);
613         }
614         else {
615                 zero_v3(adj_gravity);
616         }
617
618         /* update gravity, since this RNA setting is not part of RigidBody settings */
619         RB_dworld_set_gravity(rbw->physics_world, adj_gravity);
620
621         /* update object array in case there are changes */
622         rigidbody_update_ob_array(rbw);
623 }
624
625 static void rigidbody_update_sim_ob(Scene *scene, RigidBodyWorld *rbw, Object *ob, RigidBodyOb *rbo)
626 {
627         float loc[3];
628         float rot[4];
629         float scale[3];
630
631         /* only update if rigid body exists */
632         if (rbo->physics_object == NULL)
633                 return;
634
635         mat4_decompose(loc, rot, scale, ob->obmat);
636
637         /* update scale for all objects */
638         RB_body_set_scale(rbo->physics_object, scale);
639         /* compensate for embedded convex hull collision margin */
640         if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && rbo->shape == RB_SHAPE_CONVEXH)
641                 RB_shape_set_margin(rbo->physics_shape, RBO_GET_MARGIN(rbo) * MIN3(scale[0], scale[1], scale[2]));
642
643         /* update rigid body location and rotation for kinematic bodies */
644         if (rbo->flag & RBO_FLAG_KINEMATIC) {
645                 RB_body_activate(rbo->physics_object);
646                 RB_body_set_loc_rot(rbo->physics_object, loc, rot);
647         }
648         /* update influence of effectors - but don't do it on an effector */
649         /* only dynamic bodies need effector update */
650         else if (rbo->type == RBO_TYPE_ACTIVE && ((ob->pd == NULL) || (ob->pd->forcefield == PFIELD_NULL))) {
651                 EffectorWeights *effector_weights = rbw->effector_weights;
652                 EffectedPoint epoint;
653                 ListBase *effectors;
654
655                 /* get effectors present in the group specified by effector_weights */
656                 effectors = pdInitEffectors(scene, ob, NULL, effector_weights);
657                 if (effectors) {
658                         float force[3] = {0.0f, 0.0f, 0.0f};
659                         float loc[3], vel[3];
660
661                         /* create dummy 'point' which represents last known position of object as result of sim */
662                         // XXX: this can create some inaccuracies with sim position, but is probably better than using unsimulated vals?
663                         RB_body_get_position(rbo->physics_object, loc);
664                         RB_body_get_linear_velocity(rbo->physics_object, vel);
665
666                         pd_point_from_loc(scene, loc, vel, 0, &epoint);
667
668                         /* calculate net force of effectors, and apply to sim object
669                          *      - we use 'central force' since apply force requires a "relative position" which we don't have...
670                          */
671                         pdDoEffectors(effectors, NULL, effector_weights, &epoint, force, NULL);
672                         if (G.f & G_DEBUG)
673                                 printf("\tapplying force (%f,%f,%f) to '%s'\n", force[0], force[1], force[2], ob->id.name + 2);
674                         /* activate object in case it is deactivated */
675                         if (!is_zero_v3(force))
676                                 RB_body_activate(rbo->physics_object);
677                         RB_body_apply_central_force(rbo->physics_object, force);
678                 }
679                 else if (G.f & G_DEBUG)
680                         printf("\tno forces to apply to '%s'\n", ob->id.name + 2);
681
682                 /* cleanup */
683                 pdEndEffectors(&effectors);
684         }
685         /* NOTE: passive objects don't need to be updated since they don't move */
686
687         /* NOTE: no other settings need to be explicitly updated here,
688          * since RNA setters take care of the rest :)
689          */
690 }
691
692 /* Updates and validates world, bodies and shapes.
693  * < rebuild: rebuild entire simulation
694  */
695 static void rigidbody_update_simulation(Scene *scene, RigidBodyWorld *rbw, int rebuild)
696 {
697         GroupObject *go;
698
699         /* update world */
700         if (rebuild)
701                 BKE_rigidbody_validate_sim_world(scene, rbw, true);
702         rigidbody_update_sim_world(scene, rbw);
703
704         /* update objects */
705         for (go = rbw->group->gobject.first; go; go = go->next) {
706                 Object *ob = go->ob;
707
708                 if (ob && ob->type == OB_MESH) {
709                         /* validate that we've got valid object set up here... */
710                         RigidBodyOb *rbo = ob->rigidbody_object;
711                         /* update transformation matrix of the object so we don't get a frame of lag for simple animations */
712                         BKE_object_where_is_calc(scene, ob);
713
714                         if (rbo == NULL) {
715                                 /* Since this object is included in the sim group but doesn't have
716                                  * rigid body settings (perhaps it was added manually), add!
717                                  *      - assume object to be active? That is the default for newly added settings...
718                                  */
719                                 ob->rigidbody_object = BKE_rigidbody_create_object(scene, ob, RBO_TYPE_ACTIVE);
720                                 BKE_rigidbody_validate_sim_object(rbw, ob, true);
721
722                                 rbo = ob->rigidbody_object;
723                         }
724                         else {
725                                 /* perform simulation data updates as tagged */
726                                 /* refresh object... */
727                                 if (rebuild) {
728                                         /* World has been rebuilt so rebuild object */
729                                         BKE_rigidbody_validate_sim_object(rbw, ob, true);
730                                 }
731                                 else if (rbo->flag & RBO_FLAG_NEEDS_VALIDATE) {
732                                         BKE_rigidbody_validate_sim_object(rbw, ob, false);
733                                 }
734                                 /* refresh shape... */
735                                 if (rbo->flag & RBO_FLAG_NEEDS_RESHAPE) {
736                                         /* mesh/shape data changed, so force shape refresh */
737                                         BKE_rigidbody_validate_sim_shape(ob, true);
738                                         /* now tell RB sim about it */
739                                         // XXX: we assume that this can only get applied for active/passive shapes that will be included as rigidbodies
740                                         RB_body_set_collision_shape(rbo->physics_object, rbo->physics_shape);
741                                 }
742                                 rbo->flag &= ~(RBO_FLAG_NEEDS_VALIDATE | RBO_FLAG_NEEDS_RESHAPE);
743                         }
744
745                         /* update simulation object... */
746                         rigidbody_update_sim_ob(scene, rbw, ob, rbo);
747                 }
748         }
749 }
750
751 /* Sync rigid body and object transformations */
752 void BKE_rigidbody_sync_transforms(Scene *scene, Object *ob, float ctime)
753 {
754         RigidBodyWorld *rbw = scene->rigidbody_world;
755         RigidBodyOb *rbo = ob->rigidbody_object;
756
757         /* keep original transform for kinematic and passive objects */
758         if (ELEM(NULL, rbw, rbo) || rbo->flag & RBO_FLAG_KINEMATIC || rbo->type == RBO_TYPE_PASSIVE)
759                 return;
760
761         /* use rigid body transform after cache start frame */
762         if (ctime > rbw->pointcache->startframe) {
763                 float mat[4][4], size_mat[4][4], size[3];
764
765                 /* keep original transform when the simulation is muted */
766                 if (rbw->flag & RBW_FLAG_MUTED)
767                         return;
768
769                 normalize_qt(rbo->orn); // RB_TODO investigate why quaternion isn't normalized at this point
770                 quat_to_mat4(mat, rbo->orn);
771                 copy_v3_v3(mat[3], rbo->pos);
772
773                 mat4_to_size(size, ob->obmat);
774                 size_to_mat4(size_mat, size);
775                 mult_m4_m4m4(mat, mat, size_mat);
776
777                 copy_m4_m4(ob->obmat, mat);
778         }
779         /* otherwise set rigid body transform to current obmat */
780         else {
781                 mat4_to_loc_quat(rbo->pos, rbo->orn, ob->obmat);
782         }
783 }
784
785 void BKE_rigidbody_cache_reset(RigidBodyWorld *rbw)
786 {
787         if (rbw)
788                 rbw->pointcache->flag |= PTCACHE_OUTDATED;
789 }
790
791 /* ------------------ */
792
793 /* Run RigidBody simulation for the specified physics world */
794 void BKE_rigidbody_do_simulation(Scene *scene, float ctime)
795 {
796         float timestep;
797         RigidBodyWorld *rbw = scene->rigidbody_world;
798         PointCache *cache;
799         PTCacheID pid;
800         int startframe, endframe;
801
802         BKE_ptcache_id_from_rigidbody(&pid, NULL, rbw);
803         BKE_ptcache_id_time(&pid, scene, ctime, &startframe, &endframe, NULL);
804         cache = rbw->pointcache;
805
806         /* flag cache as outdated if we don't have a world or number of objects in the simulation has changed */
807         if (rbw->physics_world == NULL || rbw->numbodies != BLI_countlist(&rbw->group->gobject)) {
808                 cache->flag |= PTCACHE_OUTDATED;
809         }
810
811         if (ctime <= startframe) {
812                 rbw->ltime = startframe;
813                 /* reset and rebuild simulation if necessary */
814                 if (cache->flag & PTCACHE_OUTDATED) {
815                         BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
816                         rigidbody_update_simulation(scene, rbw, true);
817                         BKE_ptcache_validate(cache, (int)ctime);
818                         cache->last_exact = 0;
819                         cache->flag &= ~PTCACHE_REDO_NEEDED;
820                 }
821                 return;
822         }
823         /* rebuild world if it's outdated on second frame */
824         else if (ctime == startframe + 1 && rbw->ltime == startframe && cache->flag & PTCACHE_OUTDATED) {
825                 BKE_ptcache_id_reset(scene, &pid, PTCACHE_RESET_OUTDATED);
826                 rigidbody_update_simulation(scene, rbw, true);
827         }
828         /* make sure we don't go out of cache frame range */
829         else if (ctime > endframe) {
830                 ctime = endframe;
831         }
832
833         /* don't try to run the simulation if we don't have a world yet but allow reading baked cache */
834         if (rbw->physics_world == NULL && !(cache->flag & PTCACHE_BAKED))
835                 return;
836         else if (rbw->objects == NULL)
837                 rigidbody_update_ob_array(rbw);
838
839         /* try to read from cache */
840         // RB_TODO deal with interpolated, old and baked results
841         if (BKE_ptcache_read(&pid, ctime)) {
842                 BKE_ptcache_validate(cache, (int)ctime);
843                 rbw->ltime = ctime;
844                 return;
845         }
846
847         /* advance simulation, we can only step one frame forward */
848         if (ctime == rbw->ltime + 1) {
849                 /* write cache for first frame when on second frame */
850                 if (rbw->ltime == startframe && (cache->flag & PTCACHE_OUTDATED || cache->last_exact == 0)) {
851                         BKE_ptcache_write(&pid, startframe);
852                 }
853
854                 /* update and validate simulation */
855                 rigidbody_update_simulation(scene, rbw, false);
856
857                 /* calculate how much time elapsed since last step in seconds */
858                 timestep = 1.0f / (float)FPS * (ctime - rbw->ltime) * rbw->time_scale;
859                 /* step simulation by the requested timestep, steps per second are adjusted to take time scale into account */
860                 RB_dworld_step_simulation(rbw->physics_world, timestep, INT_MAX, 1.0f / (float)rbw->steps_per_second * min_ff(rbw->time_scale, 1.0f));
861
862                 /* write cache for current frame */
863                 BKE_ptcache_validate(cache, (int)ctime);
864                 BKE_ptcache_write(&pid, (unsigned int)ctime);
865
866                 rbw->ltime = ctime;
867         }
868 }
869 /* ************************************** */