Merge remote-tracking branch 'origin/master' into blender2.8
[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 #ifdef WITH_BULLET
46 #  include "RBI_api.h"
47 #endif
48
49 #include "DNA_group_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_cdderivedmesh.h"
57 #include "BKE_depsgraph.h"
58 #include "BKE_effect.h"
59 #include "BKE_global.h"
60 #include "BKE_library.h"
61 #include "BKE_library_query.h"
62 #include "BKE_mesh.h"
63 #include "BKE_object.h"
64 #include "BKE_rigidbody.h"
65 #include "BKE_scene.h"
66
67 /* ************************************** */
68 /* Memory Management */
69
70 /* Freeing Methods --------------------- */
71
72 #ifndef WITH_BULLET
73
74 static void RB_dworld_remove_constraint(void *UNUSED(world), void *UNUSED(con)) {}
75 static void RB_dworld_remove_body(void *UNUSED(world), void *UNUSED(body)) {}
76 static void RB_dworld_delete(void *UNUSED(world)) {}
77 static void RB_body_delete(void *UNUSED(body)) {}
78 static void RB_shape_delete(void *UNUSED(shape)) {}
79 static void RB_constraint_delete(void *UNUSED(con)) {}
80
81 #endif
82
83 /* Free rigidbody world */
84 void BKE_rigidbody_free_world(RigidBodyWorld *rbw)
85 {
86         /* sanity check */
87         if (!rbw)
88                 return;
89
90         if (rbw->physics_world) {
91                 /* free physics references, we assume that all physics objects in will have been added to the world */
92                 GroupObject *go;
93                 if (rbw->constraints) {
94                         for (go = rbw->constraints->gobject.first; go; go = go->next) {
95                                 if (go->ob && go->ob->rigidbody_constraint) {
96                                         RigidBodyCon *rbc = go->ob->rigidbody_constraint;
97
98                                         if (rbc->physics_constraint)
99                                                 RB_dworld_remove_constraint(rbw->physics_world, rbc->physics_constraint);
100                                 }
101                         }
102                 }
103                 if (rbw->group) {
104                         for (go = rbw->group->gobject.first; go; go = go->next) {
105                                 if (go->ob && go->ob->rigidbody_object) {
106                                         RigidBodyOb *rbo = go->ob->rigidbody_object;
107
108                                         if (rbo->physics_object)
109                                                 RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
110                                 }
111                         }
112                 }
113                 /* free dynamics world */
114                 RB_dworld_delete(rbw->physics_world);
115         }
116         if (rbw->objects)
117                 free(rbw->objects);
118
119         /* free effector weights */
120         if (rbw->effector_weights)
121                 MEM_freeN(rbw->effector_weights);
122
123         /* free rigidbody world itself */
124         MEM_freeN(rbw);
125 }
126
127 /* Free RigidBody settings and sim instances */
128 void BKE_rigidbody_free_object(Object *ob)
129 {
130         RigidBodyOb *rbo = (ob) ? ob->rigidbody_object : NULL;
131
132         /* sanity check */
133         if (rbo == NULL)
134                 return;
135
136         /* free physics references */
137         if (rbo->physics_object) {
138                 RB_body_delete(rbo->physics_object);
139                 rbo->physics_object = NULL;
140         }
141
142         if (rbo->physics_shape) {
143                 RB_shape_delete(rbo->physics_shape);
144                 rbo->physics_shape = NULL;
145         }
146
147         /* free data itself */
148         MEM_freeN(rbo);
149         ob->rigidbody_object = NULL;
150 }
151
152 /* Free RigidBody constraint and sim instance */
153 void BKE_rigidbody_free_constraint(Object *ob)
154 {
155         RigidBodyCon *rbc = (ob) ? ob->rigidbody_constraint : NULL;
156
157         /* sanity check */
158         if (rbc == NULL)
159                 return;
160
161         /* free physics reference */
162         if (rbc->physics_constraint) {
163                 RB_constraint_delete(rbc->physics_constraint);
164                 rbc->physics_constraint = NULL;
165         }
166
167         /* free data itself */
168         MEM_freeN(rbc);
169         ob->rigidbody_constraint = NULL;
170 }
171
172 #ifdef WITH_BULLET
173
174 /* Copying Methods --------------------- */
175
176 /* These just copy the data, clearing out references to physics objects.
177  * Anything that uses them MUST verify that the copied object will
178  * be added to relevant groups later...
179  */
180
181 RigidBodyOb *BKE_rigidbody_copy_object(Object *ob)
182 {
183         RigidBodyOb *rboN = NULL;
184
185         if (ob->rigidbody_object) {
186                 /* just duplicate the whole struct first (to catch all the settings) */
187                 rboN = MEM_dupallocN(ob->rigidbody_object);
188
189                 /* tag object as needing to be verified */
190                 rboN->flag |= RBO_FLAG_NEEDS_VALIDATE;
191
192                 /* clear out all the fields which need to be revalidated later */
193                 rboN->physics_object = NULL;
194                 rboN->physics_shape = NULL;
195         }
196
197         /* return new copy of settings */
198         return rboN;
199 }
200
201 RigidBodyCon *BKE_rigidbody_copy_constraint(Object *ob)
202 {
203         RigidBodyCon *rbcN = NULL;
204
205         if (ob->rigidbody_constraint) {
206                 /* just duplicate the whole struct first (to catch all the settings) */
207                 rbcN = MEM_dupallocN(ob->rigidbody_constraint);
208
209                 /* tag object as needing to be verified */
210                 rbcN->flag |= RBC_FLAG_NEEDS_VALIDATE;
211
212                 /* clear out all the fields which need to be revalidated later */
213                 rbcN->physics_constraint = NULL;
214         }
215
216         /* return new copy of settings */
217         return rbcN;
218 }
219
220 /* preserve relationships between constraints and rigid bodies after duplication */
221 void BKE_rigidbody_relink_constraint(RigidBodyCon *rbc)
222 {
223         ID_NEW(rbc->ob1);
224         ID_NEW(rbc->ob2);
225 }
226
227 /* ************************************** */
228 /* Setup Utilities - Validate Sim Instances */
229
230 /* get the appropriate DerivedMesh based on rigid body mesh source */
231 static DerivedMesh *rigidbody_get_mesh(Object *ob)
232 {
233         if (ob->rigidbody_object->mesh_source == RBO_MESH_DEFORM) {
234                 return ob->derivedDeform;
235         }
236         else if (ob->rigidbody_object->mesh_source == RBO_MESH_FINAL) {
237                 return ob->derivedFinal;
238         }
239         else {
240                 return CDDM_from_mesh(ob->data);
241         }
242 }
243
244 /* create collision shape of mesh - convex hull */
245 static rbCollisionShape *rigidbody_get_shape_convexhull_from_mesh(Object *ob, float margin, bool *can_embed)
246 {
247         rbCollisionShape *shape = NULL;
248         DerivedMesh *dm = NULL;
249         MVert *mvert = NULL;
250         int totvert = 0;
251
252         if (ob->type == OB_MESH && ob->data) {
253                 dm = rigidbody_get_mesh(ob);
254                 mvert   = (dm) ? dm->getVertArray(dm) : NULL;
255                 totvert = (dm) ? dm->getNumVerts(dm) : 0;
256         }
257         else {
258                 printf("ERROR: cannot make Convex Hull collision shape for non-Mesh object\n");
259         }
260
261         if (totvert) {
262                 shape = RB_shape_new_convex_hull((float *)mvert, sizeof(MVert), totvert, margin, can_embed);
263         }
264         else {
265                 printf("ERROR: no vertices to define Convex Hull collision shape with\n");
266         }
267
268         if (dm && ob->rigidbody_object->mesh_source == RBO_MESH_BASE)
269                 dm->release(dm);
270
271         return shape;
272 }
273
274 /* create collision shape of mesh - triangulated mesh
275  * returns NULL if creation fails.
276  */
277 static rbCollisionShape *rigidbody_get_shape_trimesh_from_mesh(Object *ob)
278 {
279         rbCollisionShape *shape = NULL;
280
281         if (ob->type == OB_MESH) {
282                 DerivedMesh *dm = NULL;
283                 MVert *mvert;
284                 const MLoopTri *looptri;
285                 int totvert;
286                 int tottri;
287                 const MLoop *mloop;
288                 
289                 dm = rigidbody_get_mesh(ob);
290
291                 /* ensure mesh validity, then grab data */
292                 if (dm == NULL)
293                         return NULL;
294
295                 DM_ensure_looptri(dm);
296
297                 mvert   = dm->getVertArray(dm);
298                 totvert = dm->getNumVerts(dm);
299                 looptri = dm->getLoopTriArray(dm);
300                 tottri = dm->getNumLoopTri(dm);
301                 mloop = dm->getLoopArray(dm);
302
303                 /* sanity checking - potential case when no data will be present */
304                 if ((totvert == 0) || (tottri == 0)) {
305                         printf("WARNING: no geometry data converted for Mesh Collision Shape (ob = %s)\n", ob->id.name + 2);
306                 }
307                 else {
308                         rbMeshData *mdata;
309                         int i;
310
311                         /* init mesh data for collision shape */
312                         mdata = RB_trimesh_data_new(tottri, totvert);
313                         
314                         RB_trimesh_add_vertices(mdata, (float *)mvert, totvert, sizeof(MVert));
315
316                         /* loop over all faces, adding them as triangles to the collision shape
317                          * (so for some faces, more than triangle will get added)
318                          */
319                         if (mvert && looptri) {
320                                 for (i = 0; i < tottri; i++) {
321                                         /* add first triangle - verts 1,2,3 */
322                                         const MLoopTri *lt = &looptri[i];
323                                         int vtri[3];
324
325                                         vtri[0] = mloop[lt->tri[0]].v;
326                                         vtri[1] = mloop[lt->tri[1]].v;
327                                         vtri[2] = mloop[lt->tri[2]].v;
328
329                                         RB_trimesh_add_triangle_indices(mdata, i, UNPACK3(vtri));
330                                 }
331                         }
332                         
333                         RB_trimesh_finish(mdata);
334
335                         /* construct collision shape
336                          *
337                          * These have been chosen to get better speed/accuracy tradeoffs with regards
338                          * to limitations of each:
339                          *    - BVH-Triangle Mesh: for passive objects only. Despite having greater
340                          *                         speed/accuracy, they cannot be used for moving objects.
341                          *    - GImpact Mesh:      for active objects. These are slower and less stable,
342                          *                         but are more flexible for general usage.
343                          */
344                         if (ob->rigidbody_object->type == RBO_TYPE_PASSIVE) {
345                                 shape = RB_shape_new_trimesh(mdata);
346                         }
347                         else {
348                                 shape = RB_shape_new_gimpact_mesh(mdata);
349                         }
350                 }
351
352                 /* cleanup temp data */
353                 if (ob->rigidbody_object->mesh_source == RBO_MESH_BASE) {
354                         dm->release(dm);
355                 }
356         }
357         else {
358                 printf("ERROR: cannot make Triangular Mesh collision shape for non-Mesh object\n");
359         }
360
361         return shape;
362 }
363
364 /* Create new physics sim collision shape for object and store it,
365  * or remove the existing one first and replace...
366  */
367 static void rigidbody_validate_sim_shape(Object *ob, bool rebuild)
368 {
369         RigidBodyOb *rbo = ob->rigidbody_object;
370         rbCollisionShape *new_shape = NULL;
371         BoundBox *bb = NULL;
372         float size[3] = {1.0f, 1.0f, 1.0f};
373         float radius = 1.0f;
374         float height = 1.0f;
375         float capsule_height;
376         float hull_margin = 0.0f;
377         bool can_embed = true;
378         bool has_volume;
379
380         /* sanity check */
381         if (rbo == NULL)
382                 return;
383
384         /* don't create a new shape if we already have one and don't want to rebuild it */
385         if (rbo->physics_shape && !rebuild)
386                 return;
387
388         /* if automatically determining dimensions, use the Object's boundbox
389          *      - assume that all quadrics are standing upright on local z-axis
390          *      - assume even distribution of mass around the Object's pivot
391          *        (i.e. Object pivot is centralized in boundbox)
392          */
393         // XXX: all dimensions are auto-determined now... later can add stored settings for this
394         /* get object dimensions without scaling */
395         bb = BKE_object_boundbox_get(ob);
396         if (bb) {
397                 size[0] = (bb->vec[4][0] - bb->vec[0][0]);
398                 size[1] = (bb->vec[2][1] - bb->vec[0][1]);
399                 size[2] = (bb->vec[1][2] - bb->vec[0][2]);
400         }
401         mul_v3_fl(size, 0.5f);
402
403         if (ELEM(rbo->shape, RB_SHAPE_CAPSULE, RB_SHAPE_CYLINDER, RB_SHAPE_CONE)) {
404                 /* take radius as largest x/y dimension, and height as z-dimension */
405                 radius = MAX2(size[0], size[1]);
406                 height = size[2];
407         }
408         else if (rbo->shape == RB_SHAPE_SPHERE) {
409                 /* take radius to the largest dimension to try and encompass everything */
410                 radius = MAX3(size[0], size[1], size[2]);
411         }
412
413         /* create new shape */
414         switch (rbo->shape) {
415                 case RB_SHAPE_BOX:
416                         new_shape = RB_shape_new_box(size[0], size[1], size[2]);
417                         break;
418
419                 case RB_SHAPE_SPHERE:
420                         new_shape = RB_shape_new_sphere(radius);
421                         break;
422
423                 case RB_SHAPE_CAPSULE:
424                         capsule_height = (height - radius) * 2.0f;
425                         new_shape = RB_shape_new_capsule(radius, (capsule_height > 0.0f) ? capsule_height : 0.0f);
426                         break;
427                 case RB_SHAPE_CYLINDER:
428                         new_shape = RB_shape_new_cylinder(radius, height);
429                         break;
430                 case RB_SHAPE_CONE:
431                         new_shape = RB_shape_new_cone(radius, height * 2.0f);
432                         break;
433
434                 case RB_SHAPE_CONVEXH:
435                         /* try to emged collision margin */
436                         has_volume = (MIN3(size[0], size[1], size[2]) > 0.0f);
437
438                         if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && has_volume)
439                                 hull_margin = 0.04f;
440                         new_shape = rigidbody_get_shape_convexhull_from_mesh(ob, hull_margin, &can_embed);
441                         if (!(rbo->flag & RBO_FLAG_USE_MARGIN))
442                                 rbo->margin = (can_embed && has_volume) ? 0.04f : 0.0f;  /* RB_TODO ideally we shouldn't directly change the margin here */
443                         break;
444                 case RB_SHAPE_TRIMESH:
445                         new_shape = rigidbody_get_shape_trimesh_from_mesh(ob);
446                         break;
447         }
448         /* use box shape if we can't fall back to old shape */
449         if (new_shape == NULL && rbo->physics_shape == NULL) {
450                 new_shape = RB_shape_new_box(size[0], size[1], size[2]);
451         }
452         /* assign new collision shape if creation was successful */
453         if (new_shape) {
454                 if (rbo->physics_shape)
455                         RB_shape_delete(rbo->physics_shape);
456                 rbo->physics_shape = new_shape;
457                 RB_shape_set_margin(rbo->physics_shape, RBO_GET_MARGIN(rbo));
458         }
459 }
460
461 /* --------------------- */
462
463 /* helper function to calculate volume of rigidbody object */
464 // TODO: allow a parameter to specify method used to calculate this?
465 void BKE_rigidbody_calc_volume(Object *ob, float *r_vol)
466 {
467         RigidBodyOb *rbo = ob->rigidbody_object;
468
469         float size[3]  = {1.0f, 1.0f, 1.0f};
470         float radius = 1.0f;
471         float height = 1.0f;
472
473         float volume = 0.0f;
474
475         /* if automatically determining dimensions, use the Object's boundbox
476          *      - assume that all quadrics are standing upright on local z-axis
477          *      - assume even distribution of mass around the Object's pivot
478          *        (i.e. Object pivot is centralized in boundbox)
479          *      - boundbox gives full width
480          */
481         // XXX: all dimensions are auto-determined now... later can add stored settings for this
482         BKE_object_dimensions_get(ob, size);
483
484         if (ELEM(rbo->shape, RB_SHAPE_CAPSULE, RB_SHAPE_CYLINDER, RB_SHAPE_CONE)) {
485                 /* take radius as largest x/y dimension, and height as z-dimension */
486                 radius = MAX2(size[0], size[1]) * 0.5f;
487                 height = size[2];
488         }
489         else if (rbo->shape == RB_SHAPE_SPHERE) {
490                 /* take radius to the largest dimension to try and encompass everything */
491                 radius = max_fff(size[0], size[1], size[2]) * 0.5f;
492         }
493
494         /* calculate volume as appropriate  */
495         switch (rbo->shape) {
496                 case RB_SHAPE_BOX:
497                         volume = size[0] * size[1] * size[2];
498                         break;
499
500                 case RB_SHAPE_SPHERE:
501                         volume = 4.0f / 3.0f * (float)M_PI * radius * radius * radius;
502                         break;
503
504                 /* for now, assume that capsule is close enough to a cylinder... */
505                 case RB_SHAPE_CAPSULE:
506                 case RB_SHAPE_CYLINDER:
507                         volume = (float)M_PI * radius * radius * height;
508                         break;
509
510                 case RB_SHAPE_CONE:
511                         volume = (float)M_PI / 3.0f * radius * radius * height;
512                         break;
513
514                 case RB_SHAPE_CONVEXH:
515                 case RB_SHAPE_TRIMESH:
516                 {
517                         if (ob->type == OB_MESH) {
518                                 DerivedMesh *dm = rigidbody_get_mesh(ob);
519                                 MVert *mvert;
520                                 const MLoopTri *lt = NULL;
521                                 int totvert, tottri = 0;
522                                 const MLoop *mloop = NULL;
523                                 
524                                 /* ensure mesh validity, then grab data */
525                                 if (dm == NULL)
526                                         return;
527                         
528                                 DM_ensure_looptri(dm);
529                         
530                                 mvert   = dm->getVertArray(dm);
531                                 totvert = dm->getNumVerts(dm);
532                                 lt = dm->getLoopTriArray(dm);
533                                 tottri = dm->getNumLoopTri(dm);
534                                 mloop = dm->getLoopArray(dm);
535                                 
536                                 if (totvert > 0 && tottri > 0) {
537                                         BKE_mesh_calc_volume(mvert, totvert, lt, tottri, mloop, &volume, NULL);
538                                 }
539                                 
540                                 /* cleanup temp data */
541                                 if (ob->rigidbody_object->mesh_source == RBO_MESH_BASE) {
542                                         dm->release(dm);
543                                 }
544                         }
545                         else {
546                                 /* rough estimate from boundbox as fallback */
547                                 /* XXX could implement other types of geometry here (curves, etc.) */
548                                 volume = size[0] * size[1] * size[2];
549                         }
550                         break;
551                 }
552
553 #if 0 // XXX: not defined yet
554                 case RB_SHAPE_COMPOUND:
555                         volume = 0.0f;
556                         break;
557 #endif
558         }
559
560         /* return the volume calculated */
561         if (r_vol) *r_vol = volume;
562 }
563
564 void BKE_rigidbody_calc_center_of_mass(Object *ob, float r_center[3])
565 {
566         RigidBodyOb *rbo = ob->rigidbody_object;
567
568         float size[3]  = {1.0f, 1.0f, 1.0f};
569         float height = 1.0f;
570
571         zero_v3(r_center);
572
573         /* if automatically determining dimensions, use the Object's boundbox
574          *      - assume that all quadrics are standing upright on local z-axis
575          *      - assume even distribution of mass around the Object's pivot
576          *        (i.e. Object pivot is centralized in boundbox)
577          *      - boundbox gives full width
578          */
579         // XXX: all dimensions are auto-determined now... later can add stored settings for this
580         BKE_object_dimensions_get(ob, size);
581
582         /* calculate volume as appropriate  */
583         switch (rbo->shape) {
584                 case RB_SHAPE_BOX:
585                 case RB_SHAPE_SPHERE:
586                 case RB_SHAPE_CAPSULE:
587                 case RB_SHAPE_CYLINDER:
588                         break;
589
590                 case RB_SHAPE_CONE:
591                         /* take radius as largest x/y dimension, and height as z-dimension */
592                         height = size[2];
593                         /* cone is geometrically centered on the median,
594                          * center of mass is 1/4 up from the base
595                          */
596                         r_center[2] = -0.25f * height;
597                         break;
598
599                 case RB_SHAPE_CONVEXH:
600                 case RB_SHAPE_TRIMESH:
601                 {
602                         if (ob->type == OB_MESH) {
603                                 DerivedMesh *dm = rigidbody_get_mesh(ob);
604                                 MVert *mvert;
605                                 const MLoopTri *looptri;
606                                 int totvert, tottri;
607                                 const MLoop *mloop;
608                                 
609                                 /* ensure mesh validity, then grab data */
610                                 if (dm == NULL)
611                                         return;
612                         
613                                 DM_ensure_looptri(dm);
614                         
615                                 mvert   = dm->getVertArray(dm);
616                                 totvert = dm->getNumVerts(dm);
617                                 looptri = dm->getLoopTriArray(dm);
618                                 tottri = dm->getNumLoopTri(dm);
619                                 mloop = dm->getLoopArray(dm);
620                                 
621                                 if (totvert > 0 && tottri > 0) {
622                                         BKE_mesh_calc_volume(mvert, totvert, looptri, tottri, mloop, NULL, r_center);
623                                 }
624                                 
625                                 /* cleanup temp data */
626                                 if (ob->rigidbody_object->mesh_source == RBO_MESH_BASE) {
627                                         dm->release(dm);
628                                 }
629                         }
630                         break;
631                 }
632
633 #if 0 // XXX: not defined yet
634                 case RB_SHAPE_COMPOUND:
635                         volume = 0.0f;
636                         break;
637 #endif
638         }
639 }
640
641 /* --------------------- */
642
643 /**
644  * Create physics sim representation of object given RigidBody settings
645  *
646  * \param rebuild Even if an instance already exists, replace it
647  */
648 static void rigidbody_validate_sim_object(RigidBodyWorld *rbw, Object *ob, bool rebuild)
649 {
650         RigidBodyOb *rbo = (ob) ? ob->rigidbody_object : NULL;
651         float loc[3];
652         float rot[4];
653
654         /* sanity checks:
655          *      - object doesn't have RigidBody info already: then why is it here?
656          */
657         if (rbo == NULL)
658                 return;
659
660         /* make sure collision shape exists */
661         /* FIXME we shouldn't always have to rebuild collision shapes when rebuilding objects, but it's needed for constraints to update correctly */
662         if (rbo->physics_shape == NULL || rebuild)
663                 rigidbody_validate_sim_shape(ob, true);
664
665         if (rbo->physics_object && rebuild == false) {
666                 RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
667         }
668         if (!rbo->physics_object || rebuild) {
669                 /* remove rigid body if it already exists before creating a new one */
670                 if (rbo->physics_object) {
671                         RB_body_delete(rbo->physics_object);
672                 }
673
674                 mat4_to_loc_quat(loc, rot, ob->obmat);
675
676                 rbo->physics_object = RB_body_new(rbo->physics_shape, loc, rot);
677
678                 RB_body_set_friction(rbo->physics_object, rbo->friction);
679                 RB_body_set_restitution(rbo->physics_object, rbo->restitution);
680
681                 RB_body_set_damping(rbo->physics_object, rbo->lin_damping, rbo->ang_damping);
682                 RB_body_set_sleep_thresh(rbo->physics_object, rbo->lin_sleep_thresh, rbo->ang_sleep_thresh);
683                 RB_body_set_activation_state(rbo->physics_object, rbo->flag & RBO_FLAG_USE_DEACTIVATION);
684
685                 if (rbo->type == RBO_TYPE_PASSIVE || rbo->flag & RBO_FLAG_START_DEACTIVATED)
686                         RB_body_deactivate(rbo->physics_object);
687
688
689                 RB_body_set_linear_factor(rbo->physics_object,
690                                           (ob->protectflag & OB_LOCK_LOCX) == 0,
691                                           (ob->protectflag & OB_LOCK_LOCY) == 0,
692                                           (ob->protectflag & OB_LOCK_LOCZ) == 0);
693                 RB_body_set_angular_factor(rbo->physics_object,
694                                            (ob->protectflag & OB_LOCK_ROTX) == 0,
695                                            (ob->protectflag & OB_LOCK_ROTY) == 0,
696                                            (ob->protectflag & OB_LOCK_ROTZ) == 0);
697
698                 RB_body_set_mass(rbo->physics_object, RBO_GET_MASS(rbo));
699                 RB_body_set_kinematic_state(rbo->physics_object, rbo->flag & RBO_FLAG_KINEMATIC || rbo->flag & RBO_FLAG_DISABLED);
700         }
701
702         if (rbw && rbw->physics_world)
703                 RB_dworld_add_body(rbw->physics_world, rbo->physics_object, rbo->col_groups);
704 }
705
706 /* --------------------- */
707
708 /**
709  * Create physics sim representation of constraint given rigid body constraint settings
710  *
711  * \param rebuild Even if an instance already exists, replace it
712  */
713 static void rigidbody_validate_sim_constraint(RigidBodyWorld *rbw, Object *ob, bool rebuild)
714 {
715         RigidBodyCon *rbc = (ob) ? ob->rigidbody_constraint : NULL;
716         float loc[3];
717         float rot[4];
718         float lin_lower;
719         float lin_upper;
720         float ang_lower;
721         float ang_upper;
722
723         /* sanity checks:
724          *      - object should have a rigid body constraint
725          *  - rigid body constraint should have at least one constrained object
726          */
727         if (rbc == NULL) {
728                 return;
729         }
730
731         if (ELEM(NULL, rbc->ob1, rbc->ob1->rigidbody_object, rbc->ob2, rbc->ob2->rigidbody_object)) {
732                 if (rbc->physics_constraint) {
733                         RB_dworld_remove_constraint(rbw->physics_world, rbc->physics_constraint);
734                         RB_constraint_delete(rbc->physics_constraint);
735                         rbc->physics_constraint = NULL;
736                 }
737                 return;
738         }
739
740         if (rbc->physics_constraint && rebuild == false) {
741                 RB_dworld_remove_constraint(rbw->physics_world, rbc->physics_constraint);
742         }
743         if (rbc->physics_constraint == NULL || rebuild) {
744                 rbRigidBody *rb1 = rbc->ob1->rigidbody_object->physics_object;
745                 rbRigidBody *rb2 = rbc->ob2->rigidbody_object->physics_object;
746
747                 /* remove constraint if it already exists before creating a new one */
748                 if (rbc->physics_constraint) {
749                         RB_constraint_delete(rbc->physics_constraint);
750                         rbc->physics_constraint = NULL;
751                 }
752
753                 mat4_to_loc_quat(loc, rot, ob->obmat);
754
755                 if (rb1 && rb2) {
756                         switch (rbc->type) {
757                                 case RBC_TYPE_POINT:
758                                         rbc->physics_constraint = RB_constraint_new_point(loc, rb1, rb2);
759                                         break;
760                                 case RBC_TYPE_FIXED:
761                                         rbc->physics_constraint = RB_constraint_new_fixed(loc, rot, rb1, rb2);
762                                         break;
763                                 case RBC_TYPE_HINGE:
764                                         rbc->physics_constraint = RB_constraint_new_hinge(loc, rot, rb1, rb2);
765                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_ANG_Z) {
766                                                 RB_constraint_set_limits_hinge(rbc->physics_constraint, rbc->limit_ang_z_lower, rbc->limit_ang_z_upper);
767                                         }
768                                         else
769                                                 RB_constraint_set_limits_hinge(rbc->physics_constraint, 0.0f, -1.0f);
770                                         break;
771                                 case RBC_TYPE_SLIDER:
772                                         rbc->physics_constraint = RB_constraint_new_slider(loc, rot, rb1, rb2);
773                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_LIN_X)
774                                                 RB_constraint_set_limits_slider(rbc->physics_constraint, rbc->limit_lin_x_lower, rbc->limit_lin_x_upper);
775                                         else
776                                                 RB_constraint_set_limits_slider(rbc->physics_constraint, 0.0f, -1.0f);
777                                         break;
778                                 case RBC_TYPE_PISTON:
779                                         rbc->physics_constraint = RB_constraint_new_piston(loc, rot, rb1, rb2);
780                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_LIN_X) {
781                                                 lin_lower = rbc->limit_lin_x_lower;
782                                                 lin_upper = rbc->limit_lin_x_upper;
783                                         }
784                                         else {
785                                                 lin_lower = 0.0f;
786                                                 lin_upper = -1.0f;
787                                         }
788                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_ANG_X) {
789                                                 ang_lower = rbc->limit_ang_x_lower;
790                                                 ang_upper = rbc->limit_ang_x_upper;
791                                         }
792                                         else {
793                                                 ang_lower = 0.0f;
794                                                 ang_upper = -1.0f;
795                                         }
796                                         RB_constraint_set_limits_piston(rbc->physics_constraint, lin_lower, lin_upper, ang_lower, ang_upper);
797                                         break;
798                                 case RBC_TYPE_6DOF_SPRING:
799                                         rbc->physics_constraint = RB_constraint_new_6dof_spring(loc, rot, rb1, rb2);
800
801                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_X, rbc->flag & RBC_FLAG_USE_SPRING_X);
802                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_X, rbc->spring_stiffness_x);
803                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_X, rbc->spring_damping_x);
804
805                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Y, rbc->flag & RBC_FLAG_USE_SPRING_Y);
806                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Y, rbc->spring_stiffness_y);
807                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Y, rbc->spring_damping_y);
808
809                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Z, rbc->flag & RBC_FLAG_USE_SPRING_Z);
810                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Z, rbc->spring_stiffness_z);
811                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_LIN_Z, rbc->spring_damping_z);
812
813                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_X, rbc->flag & RBC_FLAG_USE_SPRING_ANG_X);
814                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_X, rbc->spring_stiffness_ang_x);
815                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_X, rbc->spring_damping_ang_x);
816
817                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Y, rbc->flag & RBC_FLAG_USE_SPRING_ANG_Y);
818                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Y, rbc->spring_stiffness_ang_y);
819                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Y, rbc->spring_damping_ang_y);
820
821                                         RB_constraint_set_spring_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Z, rbc->flag & RBC_FLAG_USE_SPRING_ANG_Z);
822                                         RB_constraint_set_stiffness_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Z, rbc->spring_stiffness_ang_z);
823                                         RB_constraint_set_damping_6dof_spring(rbc->physics_constraint, RB_LIMIT_ANG_Z, rbc->spring_damping_ang_z);
824
825                                         RB_constraint_set_equilibrium_6dof_spring(rbc->physics_constraint);
826                                         /* fall-through */
827                                 case RBC_TYPE_6DOF:
828                                         if (rbc->type == RBC_TYPE_6DOF) /* a litte awkward but avoids duplicate code for limits */
829                                                 rbc->physics_constraint = RB_constraint_new_6dof(loc, rot, rb1, rb2);
830
831                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_LIN_X)
832                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_X, rbc->limit_lin_x_lower, rbc->limit_lin_x_upper);
833                                         else
834                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_X, 0.0f, -1.0f);
835
836                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_LIN_Y)
837                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_Y, rbc->limit_lin_y_lower, rbc->limit_lin_y_upper);
838                                         else
839                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_Y, 0.0f, -1.0f);
840
841                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_LIN_Z)
842                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_Z, rbc->limit_lin_z_lower, rbc->limit_lin_z_upper);
843                                         else
844                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_LIN_Z, 0.0f, -1.0f);
845
846                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_ANG_X)
847                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_X, rbc->limit_ang_x_lower, rbc->limit_ang_x_upper);
848                                         else
849                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_X, 0.0f, -1.0f);
850
851                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_ANG_Y)
852                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_Y, rbc->limit_ang_y_lower, rbc->limit_ang_y_upper);
853                                         else
854                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_Y, 0.0f, -1.0f);
855
856                                         if (rbc->flag & RBC_FLAG_USE_LIMIT_ANG_Z)
857                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_Z, rbc->limit_ang_z_lower, rbc->limit_ang_z_upper);
858                                         else
859                                                 RB_constraint_set_limits_6dof(rbc->physics_constraint, RB_LIMIT_ANG_Z, 0.0f, -1.0f);
860                                         break;
861                                 case RBC_TYPE_MOTOR:
862                                         rbc->physics_constraint = RB_constraint_new_motor(loc, rot, rb1, rb2);
863
864                                         RB_constraint_set_enable_motor(rbc->physics_constraint, rbc->flag & RBC_FLAG_USE_MOTOR_LIN, rbc->flag & RBC_FLAG_USE_MOTOR_ANG);
865                                         RB_constraint_set_max_impulse_motor(rbc->physics_constraint, rbc->motor_lin_max_impulse, rbc->motor_ang_max_impulse);
866                                         RB_constraint_set_target_velocity_motor(rbc->physics_constraint, rbc->motor_lin_target_velocity, rbc->motor_ang_target_velocity);
867                                         break;
868                         }
869                 }
870                 else { /* can't create constraint without both rigid bodies */
871                         return;
872                 }
873
874                 RB_constraint_set_enabled(rbc->physics_constraint, rbc->flag & RBC_FLAG_ENABLED);
875
876                 if (rbc->flag & RBC_FLAG_USE_BREAKING)
877                         RB_constraint_set_breaking_threshold(rbc->physics_constraint, rbc->breaking_threshold);
878                 else
879                         RB_constraint_set_breaking_threshold(rbc->physics_constraint, FLT_MAX);
880
881                 if (rbc->flag & RBC_FLAG_OVERRIDE_SOLVER_ITERATIONS)
882                         RB_constraint_set_solver_iterations(rbc->physics_constraint, rbc->num_solver_iterations);
883                 else
884                         RB_constraint_set_solver_iterations(rbc->physics_constraint, -1);
885         }
886
887         if (rbw && rbw->physics_world && rbc->physics_constraint) {
888                 RB_dworld_add_constraint(rbw->physics_world, rbc->physics_constraint, rbc->flag & RBC_FLAG_DISABLE_COLLISIONS);
889         }
890 }
891
892 /* --------------------- */
893
894 /* Create physics sim world given RigidBody world settings */
895 // NOTE: this does NOT update object references that the scene uses, in case those aren't ready yet!
896 void BKE_rigidbody_validate_sim_world(Scene *scene, RigidBodyWorld *rbw, bool rebuild)
897 {
898         /* sanity checks */
899         if (rbw == NULL)
900                 return;
901
902         /* create new sim world */
903         if (rebuild || rbw->physics_world == NULL) {
904                 if (rbw->physics_world)
905                         RB_dworld_delete(rbw->physics_world);
906                 rbw->physics_world = RB_dworld_new(scene->physics_settings.gravity);
907         }
908
909         RB_dworld_set_solver_iterations(rbw->physics_world, rbw->num_solver_iterations);
910         RB_dworld_set_split_impulse(rbw->physics_world, rbw->flag & RBW_FLAG_USE_SPLIT_IMPULSE);
911 }
912
913 /* ************************************** */
914 /* Setup Utilities - Create Settings Blocks */
915
916 /* Set up RigidBody world */
917 RigidBodyWorld *BKE_rigidbody_create_world(Scene *scene)
918 {
919         /* try to get whatever RigidBody world that might be representing this already */
920         RigidBodyWorld *rbw;
921
922         /* sanity checks
923          *      - there must be a valid scene to add world to
924          *      - there mustn't be a sim world using this group already
925          */
926         if (scene == NULL)
927                 return NULL;
928
929         /* create a new sim world */
930         rbw = MEM_callocN(sizeof(RigidBodyWorld), "RigidBodyWorld");
931
932         /* set default settings */
933         rbw->effector_weights = BKE_add_effector_weights(NULL);
934
935         rbw->ltime = PSFRA;
936
937         rbw->time_scale = 1.0f;
938
939         rbw->steps_per_second = 60; /* Bullet default (60 Hz) */
940         rbw->num_solver_iterations = 10; /* 10 is bullet default */
941
942         /* return this sim world */
943         return rbw;
944 }
945
946 RigidBodyWorld *BKE_rigidbody_world_copy(RigidBodyWorld *rbw)
947 {
948         RigidBodyWorld *rbwn = MEM_dupallocN(rbw);
949
950         if (rbw->effector_weights)
951                 rbwn->effector_weights = MEM_dupallocN(rbw->effector_weights);
952         if (rbwn->group)
953                 id_us_plus(&rbwn->group->id);
954         if (rbwn->constraints)
955                 id_us_plus(&rbwn->constraints->id);
956
957         rbwn->objects = NULL;
958         rbwn->physics_world = NULL;
959         rbwn->numbodies = 0;
960
961         return rbwn;
962 }
963
964 void BKE_rigidbody_world_groups_relink(RigidBodyWorld *rbw)
965 {
966         if (rbw->group && rbw->group->id.newid)
967                 rbw->group = (Group *)rbw->group->id.newid;
968         if (rbw->constraints && rbw->constraints->id.newid)
969                 rbw->constraints = (Group *)rbw->constraints->id.newid;
970         if (rbw->effector_weights->group && rbw->effector_weights->group->id.newid)
971                 rbw->effector_weights->group = (Group *)rbw->effector_weights->group->id.newid;
972 }
973
974 void BKE_rigidbody_world_id_loop(RigidBodyWorld *rbw, RigidbodyWorldIDFunc func, void *userdata)
975 {
976         func(rbw, (ID **)&rbw->group, userdata, IDWALK_NOP);
977         func(rbw, (ID **)&rbw->constraints, userdata, IDWALK_NOP);
978         func(rbw, (ID **)&rbw->effector_weights->group, userdata, IDWALK_NOP);
979
980         if (rbw->objects) {
981                 int i;
982                 for (i = 0; i < rbw->numbodies; i++) {
983                         func(rbw, (ID **)&rbw->objects[i], userdata, IDWALK_NOP);
984                 }
985         }
986 }
987
988 /* Add rigid body settings to the specified object */
989 RigidBodyOb *BKE_rigidbody_create_object(Scene *UNUSED(scene), Object *ob, short type)
990 {
991         RigidBodyOb *rbo;
992
993         /* sanity checks
994          *      - rigidbody world must exist
995          *      - object must exist
996          *      - cannot add rigid body if it already exists
997          */
998         if (ob == NULL || (ob->rigidbody_object != NULL))
999                 return NULL;
1000
1001         /* create new settings data, and link it up */
1002         rbo = MEM_callocN(sizeof(RigidBodyOb), "RigidBodyOb");
1003
1004         /* set default settings */
1005         rbo->type = type;
1006
1007         rbo->mass = 1.0f;
1008
1009         rbo->friction = 0.5f; /* best when non-zero. 0.5 is Bullet default */
1010         rbo->restitution = 0.0f; /* best when zero. 0.0 is Bullet default */
1011
1012         rbo->margin = 0.04f; /* 0.04 (in meters) is Bullet default */
1013
1014         rbo->lin_sleep_thresh = 0.4f; /* 0.4 is half of Bullet default */
1015         rbo->ang_sleep_thresh = 0.5f; /* 0.5 is half of Bullet default */
1016
1017         rbo->lin_damping = 0.04f; /* 0.04 is game engine default */
1018         rbo->ang_damping = 0.1f; /* 0.1 is game engine default */
1019
1020         rbo->col_groups = 1;
1021
1022         /* use triangle meshes for passive objects
1023          * use convex hulls for active objects since dynamic triangle meshes are very unstable
1024          */
1025         if (type == RBO_TYPE_ACTIVE)
1026                 rbo->shape = RB_SHAPE_CONVEXH;
1027         else
1028                 rbo->shape = RB_SHAPE_TRIMESH;
1029
1030         rbo->mesh_source = RBO_MESH_DEFORM;
1031
1032         /* set initial transform */
1033         mat4_to_loc_quat(rbo->pos, rbo->orn, ob->obmat);
1034
1035         /* return this object */
1036         return rbo;
1037 }
1038
1039 /* Add rigid body constraint to the specified object */
1040 RigidBodyCon *BKE_rigidbody_create_constraint(Scene *UNUSED(scene), Object *ob, short type)
1041 {
1042         RigidBodyCon *rbc;
1043
1044         /* sanity checks
1045          *      - rigidbody world must exist
1046          *      - object must exist
1047          *      - cannot add constraint if it already exists
1048          */
1049         if (ob == NULL || (ob->rigidbody_constraint != NULL))
1050                 return NULL;
1051
1052         /* create new settings data, and link it up */
1053         rbc = MEM_callocN(sizeof(RigidBodyCon), "RigidBodyCon");
1054
1055         /* set default settings */
1056         rbc->type = type;
1057
1058         rbc->ob1 = NULL;
1059         rbc->ob2 = NULL;
1060
1061         rbc->flag |= RBC_FLAG_ENABLED;
1062         rbc->flag |= RBC_FLAG_DISABLE_COLLISIONS;
1063
1064         rbc->breaking_threshold = 10.0f; /* no good default here, just use 10 for now */
1065         rbc->num_solver_iterations = 10; /* 10 is Bullet default */
1066
1067         rbc->limit_lin_x_lower = -1.0f;
1068         rbc->limit_lin_x_upper = 1.0f;
1069         rbc->limit_lin_y_lower = -1.0f;
1070         rbc->limit_lin_y_upper = 1.0f;
1071         rbc->limit_lin_z_lower = -1.0f;
1072         rbc->limit_lin_z_upper = 1.0f;
1073         rbc->limit_ang_x_lower = -M_PI_4;
1074         rbc->limit_ang_x_upper = M_PI_4;
1075         rbc->limit_ang_y_lower = -M_PI_4;
1076         rbc->limit_ang_y_upper = M_PI_4;
1077         rbc->limit_ang_z_lower = -M_PI_4;
1078         rbc->limit_ang_z_upper = M_PI_4;
1079
1080         rbc->spring_damping_x = 0.5f;
1081         rbc->spring_damping_y = 0.5f;
1082         rbc->spring_damping_z = 0.5f;
1083         rbc->spring_damping_ang_x = 0.5f;
1084         rbc->spring_damping_ang_y = 0.5f;
1085         rbc->spring_damping_ang_z = 0.5f;
1086         rbc->spring_stiffness_x = 10.0f;
1087         rbc->spring_stiffness_y = 10.0f;
1088         rbc->spring_stiffness_z = 10.0f;
1089         rbc->spring_stiffness_ang_x = 10.0f;
1090         rbc->spring_stiffness_ang_y = 10.0f;
1091         rbc->spring_stiffness_ang_z = 10.0f;
1092
1093         rbc->motor_lin_max_impulse = 1.0f;
1094         rbc->motor_lin_target_velocity = 1.0f;
1095         rbc->motor_ang_max_impulse = 1.0f;
1096         rbc->motor_ang_target_velocity = 1.0f;
1097
1098         /* return this object */
1099         return rbc;
1100 }
1101
1102 /* ************************************** */
1103 /* Utilities API */
1104
1105 /* Get RigidBody world for the given scene, creating one if needed
1106  *
1107  * \param scene Scene to find active Rigid Body world for
1108  */
1109 RigidBodyWorld *BKE_rigidbody_get_world(Scene *scene)
1110 {
1111         /* sanity check */
1112         if (scene == NULL)
1113                 return NULL;
1114
1115         return scene->rigidbody_world;
1116 }
1117
1118 void BKE_rigidbody_remove_object(Scene *scene, Object *ob)
1119 {
1120         RigidBodyWorld *rbw = scene->rigidbody_world;
1121         RigidBodyOb *rbo = ob->rigidbody_object;
1122         RigidBodyCon *rbc;
1123         GroupObject *go;
1124         int i;
1125
1126         if (rbw) {
1127                 /* remove from rigidbody world, free object won't do this */
1128                 if (rbw->physics_world && rbo->physics_object)
1129                         RB_dworld_remove_body(rbw->physics_world, rbo->physics_object);
1130
1131                 /* remove object from array */
1132                 if (rbw && rbw->objects) {
1133                         for (i = 0; i < rbw->numbodies; i++) {
1134                                 if (rbw->objects[i] == ob) {
1135                                         rbw->objects[i] = NULL;
1136                                         break;
1137                                 }
1138                         }
1139                 }
1140
1141                 /* remove object from rigid body constraints */
1142                 if (rbw->constraints) {
1143                         for (go = rbw->constraints->gobject.first; go; go = go->next) {
1144                                 Object *obt = go->ob;
1145                                 if (obt && obt->rigidbody_constraint) {
1146                                         rbc = obt->rigidbody_constraint;
1147                                         if (ELEM(ob, rbc->ob1, rbc->ob2)) {
1148                                                 BKE_rigidbody_remove_constraint(scene, obt);
1149                                         }
1150                                 }
1151                         }
1152                 }
1153         }
1154
1155         /* remove object's settings */
1156         BKE_rigidbody_free_object(ob);
1157 }
1158
1159 void BKE_rigidbody_remove_constraint(Scene *scene, Object *ob)
1160 {
1161         RigidBodyWorld *rbw = scene->rigidbody_world;
1162         RigidBodyCon *rbc = ob->rigidbody_constraint;
1163
1164         /* remove from rigidbody world, free object won't do this */
1165         if (rbw && rbw->physics_world && rbc->physics_constraint) {
1166                 RB_dworld_remove_constraint(rbw->physics_world, rbc->physics_constraint);
1167         }
1168         /* remove object's settings */
1169         BKE_rigidbody_free_constraint(ob);
1170 }
1171
1172
1173 /* ************************************** */
1174 /* Simulation Interface - Bullet */
1175
1176 /* Update object array and rigid body count so they're in sync with the rigid body group */
1177 static void rigidbody_update_ob_array(RigidBodyWorld *rbw)
1178 {
1179         GroupObject *go;
1180         int i, n;
1181
1182         n = BLI_listbase_count(&rbw->group->gobject);
1183
1184         if (rbw->numbodies != n) {
1185                 rbw->numbodies = n;
1186                 rbw->objects = realloc(rbw->objects, sizeof(Object *) * rbw->numbodies);
1187         }
1188
1189         for (go = rbw->group->gobject.first, i = 0; go; go = go->next, i++) {
1190                 Object *ob = go->ob;
1191                 rbw->objects[i] = ob;
1192         }
1193 }
1194
1195 static void rigidbody_update_sim_world(Scene *scene, RigidBodyWorld *rbw)
1196 {
1197         float adj_gravity[3];
1198
1199         /* adjust gravity to take effector weights into account */
1200         if (scene->physics_settings.flag & PHYS_GLOBAL_GRAVITY) {
1201                 copy_v3_v3(adj_gravity, scene->physics_settings.gravity);
1202                 mul_v3_fl(adj_gravity, rbw->effector_weights->global_gravity * rbw->effector_weights->weight[0]);
1203         }
1204         else {
1205                 zero_v3(adj_gravity);
1206         }
1207
1208         /* update gravity, since this RNA setting is not part of RigidBody settings */
1209         RB_dworld_set_gravity(rbw->physics_world, adj_gravity);
1210
1211         /* update object array in case there are changes */
1212         rigidbody_update_ob_array(rbw);
1213 }
1214
1215 static void rigidbody_update_sim_ob(Scene *scene, RigidBodyWorld *rbw, Object *ob, RigidBodyOb *rbo)
1216 {
1217         float loc[3];
1218         float rot[4];
1219         float scale[3];
1220
1221         /* only update if rigid body exists */
1222         if (rbo->physics_object == NULL)
1223                 return;
1224
1225         if (rbo->shape == RB_SHAPE_TRIMESH && rbo->flag & RBO_FLAG_USE_DEFORM) {
1226                 DerivedMesh *dm = ob->derivedDeform;
1227                 if (dm) {
1228                         MVert *mvert = dm->getVertArray(dm);
1229                         int totvert = dm->getNumVerts(dm);
1230                         BoundBox *bb = BKE_object_boundbox_get(ob);
1231
1232                         RB_shape_trimesh_update(rbo->physics_shape, (float *)mvert, totvert, sizeof(MVert), bb->vec[0], bb->vec[6]);
1233                 }
1234         }
1235
1236         mat4_decompose(loc, rot, scale, ob->obmat);
1237
1238         /* update scale for all objects */
1239         RB_body_set_scale(rbo->physics_object, scale);
1240         /* compensate for embedded convex hull collision margin */
1241         if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && rbo->shape == RB_SHAPE_CONVEXH)
1242                 RB_shape_set_margin(rbo->physics_shape, RBO_GET_MARGIN(rbo) * MIN3(scale[0], scale[1], scale[2]));
1243
1244         /* make transformed objects temporarily kinmatic so that they can be moved by the user during simulation */
1245         if (ob->flag & SELECT && G.moving & G_TRANSFORM_OBJ) {
1246                 RB_body_set_kinematic_state(rbo->physics_object, true);
1247                 RB_body_set_mass(rbo->physics_object, 0.0f);
1248         }
1249
1250         /* update rigid body location and rotation for kinematic bodies */
1251         if (rbo->flag & RBO_FLAG_KINEMATIC || (ob->flag & SELECT && G.moving & G_TRANSFORM_OBJ)) {
1252                 RB_body_activate(rbo->physics_object);
1253                 RB_body_set_loc_rot(rbo->physics_object, loc, rot);
1254         }
1255         /* update influence of effectors - but don't do it on an effector */
1256         /* only dynamic bodies need effector update */
1257         else if (rbo->type == RBO_TYPE_ACTIVE && ((ob->pd == NULL) || (ob->pd->forcefield == PFIELD_NULL))) {
1258                 EffectorWeights *effector_weights = rbw->effector_weights;
1259                 EffectedPoint epoint;
1260                 ListBase *effectors;
1261
1262                 /* get effectors present in the group specified by effector_weights */
1263                 effectors = pdInitEffectors(scene, ob, effector_weights, true);
1264                 if (effectors) {
1265                         float eff_force[3] = {0.0f, 0.0f, 0.0f};
1266                         float eff_loc[3], eff_vel[3];
1267
1268                         /* create dummy 'point' which represents last known position of object as result of sim */
1269                         // XXX: this can create some inaccuracies with sim position, but is probably better than using unsimulated vals?
1270                         RB_body_get_position(rbo->physics_object, eff_loc);
1271                         RB_body_get_linear_velocity(rbo->physics_object, eff_vel);
1272
1273                         pd_point_from_loc(scene, eff_loc, eff_vel, 0, &epoint);
1274
1275                         /* calculate net force of effectors, and apply to sim object
1276                          *      - we use 'central force' since apply force requires a "relative position" which we don't have...
1277                          */
1278                         pdDoEffectors(effectors, NULL, effector_weights, &epoint, eff_force, NULL);
1279                         if (G.f & G_DEBUG)
1280                                 printf("\tapplying force (%f,%f,%f) to '%s'\n", eff_force[0], eff_force[1], eff_force[2], ob->id.name + 2);
1281                         /* activate object in case it is deactivated */
1282                         if (!is_zero_v3(eff_force))
1283                                 RB_body_activate(rbo->physics_object);
1284                         RB_body_apply_central_force(rbo->physics_object, eff_force);
1285                 }
1286                 else if (G.f & G_DEBUG)
1287                         printf("\tno forces to apply to '%s'\n", ob->id.name + 2);
1288
1289                 /* cleanup */
1290                 pdEndEffectors(&effectors);
1291         }
1292         /* NOTE: passive objects don't need to be updated since they don't move */
1293
1294         /* NOTE: no other settings need to be explicitly updated here,
1295          * since RNA setters take care of the rest :)
1296          */
1297 }
1298
1299 /**
1300  * Updates and validates world, bodies and shapes.
1301  *
1302  * \param rebuild Rebuild entire simulation
1303  */
1304 static void rigidbody_update_simulation(Scene *scene, RigidBodyWorld *rbw, bool rebuild)
1305 {
1306         GroupObject *go;
1307
1308         /* update world */
1309         if (rebuild)
1310                 BKE_rigidbody_validate_sim_world(scene, rbw, true);
1311         rigidbody_update_sim_world(scene, rbw);
1312
1313         /* XXX TODO For rebuild: remove all constraints first.
1314          * Otherwise we can end up deleting objects that are still
1315          * referenced by constraints, corrupting bullet's internal list.
1316          * 
1317          * Memory management needs redesign here, this is just a dirty workaround.
1318          */
1319         if (rebuild && rbw->constraints) {
1320                 for (go = rbw->constraints->gobject.first; go; go = go->next) {
1321                         Object *ob = go->ob;
1322                         if (ob) {
1323                                 RigidBodyCon *rbc = ob->rigidbody_constraint;
1324                                 if (rbc && rbc->physics_constraint) {
1325                                         RB_dworld_remove_constraint(rbw->physics_world, rbc->physics_constraint);
1326                                         RB_constraint_delete(rbc->physics_constraint);
1327                                         rbc->physics_constraint = NULL;
1328                                 }
1329                         }
1330                 }
1331         }
1332
1333         /* update objects */
1334         for (go = rbw->group->gobject.first; go; go = go->next) {
1335                 Object *ob = go->ob;
1336
1337                 if (ob && ob->type == OB_MESH) {
1338                         /* validate that we've got valid object set up here... */
1339                         RigidBodyOb *rbo = ob->rigidbody_object;
1340                         /* update transformation matrix of the object so we don't get a frame of lag for simple animations */
1341                         BKE_object_where_is_calc(scene, ob);
1342
1343                         if (rbo == NULL) {
1344                                 /* Since this object is included in the sim group but doesn't have
1345                                  * rigid body settings (perhaps it was added manually), add!
1346                                  *      - assume object to be active? That is the default for newly added settings...
1347                                  */
1348                                 ob->rigidbody_object = BKE_rigidbody_create_object(scene, ob, RBO_TYPE_ACTIVE);
1349                                 rigidbody_validate_sim_object(rbw, ob, true);
1350
1351                                 rbo = ob->rigidbody_object;
1352                         }
1353                         else {
1354                                 /* perform simulation data updates as tagged */
1355                                 /* refresh object... */
1356                                 if (rebuild) {
1357                                         /* World has been rebuilt so rebuild object */
1358                                         rigidbody_validate_sim_object(rbw, ob, true);
1359                                 }
1360                                 else if (rbo->flag & RBO_FLAG_NEEDS_VALIDATE) {
1361                                         rigidbody_validate_sim_object(rbw, ob, false);
1362                                 }
1363                                 /* refresh shape... */
1364                                 if (rbo->flag & RBO_FLAG_NEEDS_RESHAPE) {
1365                                         /* mesh/shape data changed, so force shape refresh */
1366                                         rigidbody_validate_sim_shape(ob, true);
1367                                         /* now tell RB sim about it */
1368                                         // XXX: we assume that this can only get applied for active/passive shapes that will be included as rigidbodies
1369                                         RB_body_set_collision_shape(rbo->physics_object, rbo->physics_shape);
1370                                 }
1371                                 rbo->flag &= ~(RBO_FLAG_NEEDS_VALIDATE | RBO_FLAG_NEEDS_RESHAPE);
1372                         }
1373
1374                         /* update simulation object... */
1375                         rigidbody_update_sim_ob(scene, rbw, ob, rbo);
1376                 }
1377         }
1378         
1379         /* update constraints */
1380         if (rbw->constraints == NULL) /* no constraints, move on */
1381                 return;
1382         for (go = rbw->constraints->gobject.first; go; go = go->next) {
1383                 Object *ob = go->ob;
1384
1385                 if (ob) {
1386                         /* validate that we've got valid object set up here... */
1387                         RigidBodyCon *rbc = ob->rigidbody_constraint;
1388                         /* update transformation matrix of the object so we don't get a frame of lag for simple animations */
1389                         BKE_object_where_is_calc(scene, ob);
1390
1391                         if (rbc == NULL) {
1392                                 /* Since this object is included in the group but doesn't have
1393                                  * constraint settings (perhaps it was added manually), add!
1394                                  */
1395                                 ob->rigidbody_constraint = BKE_rigidbody_create_constraint(scene, ob, RBC_TYPE_FIXED);
1396                                 rigidbody_validate_sim_constraint(rbw, ob, true);
1397
1398                                 rbc = ob->rigidbody_constraint;
1399                         }
1400                         else {
1401                                 /* perform simulation data updates as tagged */
1402                                 if (rebuild) {
1403                                         /* World has been rebuilt so rebuild constraint */
1404                                         rigidbody_validate_sim_constraint(rbw, ob, true);
1405                                 }
1406                                 else if (rbc->flag & RBC_FLAG_NEEDS_VALIDATE) {
1407                                         rigidbody_validate_sim_constraint(rbw, ob, false);
1408                                 }
1409                                 rbc->flag &= ~RBC_FLAG_NEEDS_VALIDATE;
1410                         }
1411                 }
1412         }
1413 }
1414
1415 static void rigidbody_update_simulation_post_step(RigidBodyWorld *rbw)
1416 {
1417         GroupObject *go;
1418
1419         for (go = rbw->group->gobject.first; go; go = go->next) {
1420                 Object *ob = go->ob;
1421
1422                 if (ob) {
1423                         RigidBodyOb *rbo = ob->rigidbody_object;
1424                         /* reset kinematic state for transformed objects */
1425                         if (rbo && (ob->flag & SELECT) && (G.moving & G_TRANSFORM_OBJ)) {
1426                                 RB_body_set_kinematic_state(rbo->physics_object, rbo->flag & RBO_FLAG_KINEMATIC || rbo->flag & RBO_FLAG_DISABLED);
1427                                 RB_body_set_mass(rbo->physics_object, RBO_GET_MASS(rbo));
1428                                 /* deactivate passive objects so they don't interfere with deactivation of active objects */
1429                                 if (rbo->type == RBO_TYPE_PASSIVE)
1430                                         RB_body_deactivate(rbo->physics_object);
1431                         }
1432                 }
1433         }
1434 }
1435
1436 bool BKE_rigidbody_check_sim_running(RigidBodyWorld *rbw, float UNUSED(ctime))
1437 {
1438         return (rbw && (rbw->flag & RBW_FLAG_MUTED) == 0);
1439 }
1440
1441 /* Sync rigid body and object transformations */
1442 void BKE_rigidbody_sync_transforms(RigidBodyWorld *rbw, Object *ob, float ctime)
1443 {
1444         RigidBodyOb *rbo = ob->rigidbody_object;
1445
1446         /* keep original transform for kinematic and passive objects */
1447         if (ELEM(NULL, rbw, rbo) || rbo->flag & RBO_FLAG_KINEMATIC || rbo->type == RBO_TYPE_PASSIVE)
1448                 return;
1449
1450         /* use rigid body transform after cache start frame if objects is not being transformed */
1451         if (BKE_rigidbody_check_sim_running(rbw, ctime) && !(ob->flag & SELECT && G.moving & G_TRANSFORM_OBJ)) {
1452                 float mat[4][4], size_mat[4][4], size[3];
1453
1454                 normalize_qt(rbo->orn); // RB_TODO investigate why quaternion isn't normalized at this point
1455                 quat_to_mat4(mat, rbo->orn);
1456                 copy_v3_v3(mat[3], rbo->pos);
1457
1458                 mat4_to_size(size, ob->obmat);
1459                 size_to_mat4(size_mat, size);
1460                 mul_m4_m4m4(mat, mat, size_mat);
1461
1462                 copy_m4_m4(ob->obmat, mat);
1463         }
1464         /* otherwise set rigid body transform to current obmat */
1465         else {
1466                 mat4_to_loc_quat(rbo->pos, rbo->orn, ob->obmat);
1467         }
1468 }
1469
1470 /* Used when canceling transforms - return rigidbody and object to initial states */
1471 void BKE_rigidbody_aftertrans_update(Object *ob, float loc[3], float rot[3], float quat[4], float rotAxis[3], float rotAngle)
1472 {
1473         RigidBodyOb *rbo = ob->rigidbody_object;
1474
1475         /* return rigid body and object to their initial states */
1476         copy_v3_v3(rbo->pos, ob->loc);
1477         copy_v3_v3(ob->loc, loc);
1478
1479         if (ob->rotmode > 0) {
1480                 eulO_to_quat(rbo->orn, ob->rot, ob->rotmode);
1481                 copy_v3_v3(ob->rot, rot);
1482         }
1483         else if (ob->rotmode == ROT_MODE_AXISANGLE) {
1484                 axis_angle_to_quat(rbo->orn, ob->rotAxis, ob->rotAngle);
1485                 copy_v3_v3(ob->rotAxis, rotAxis);
1486                 ob->rotAngle = rotAngle;
1487         }
1488         else {
1489                 copy_qt_qt(rbo->orn, ob->quat);
1490                 copy_qt_qt(ob->quat, quat);
1491         }
1492         if (rbo->physics_object) {
1493                 /* allow passive objects to return to original transform */
1494                 if (rbo->type == RBO_TYPE_PASSIVE)
1495                         RB_body_set_kinematic_state(rbo->physics_object, true);
1496                 RB_body_set_loc_rot(rbo->physics_object, rbo->pos, rbo->orn);
1497         }
1498         // RB_TODO update rigid body physics object's loc/rot for dynamic objects here as well (needs to be done outside bullet's update loop)
1499 }
1500
1501 /* ------------------ */
1502
1503 /* Rebuild rigid body world */
1504 /* NOTE: this needs to be called before frame update to work correctly */
1505 void BKE_rigidbody_rebuild_world(Scene *scene, float ctime)
1506 {
1507         RigidBodyWorld *rbw = scene->rigidbody_world;
1508         int startframe = scene->r.sfra;
1509
1510         if (ctime == startframe + 1 && rbw->ltime == startframe) {
1511                 rigidbody_update_simulation(scene, rbw, true);
1512         }
1513 }
1514
1515 /* Run RigidBody simulation for the specified physics world */
1516 void BKE_rigidbody_do_simulation(Scene *scene, float ctime)
1517 {
1518         float timestep;
1519         RigidBodyWorld *rbw = scene->rigidbody_world;
1520         int startframe = scene->r.sfra, endframe = scene->r.efra;
1521
1522         if (ctime <= startframe) {
1523                 rbw->ltime = startframe;
1524                 return;
1525         }
1526         /* make sure we don't go out of cache frame range */
1527         else if (ctime > endframe) {
1528                 ctime = endframe;
1529         }
1530
1531         /* don't try to run the simulation if we don't have a world yet */
1532         if (rbw->physics_world == NULL)
1533                 return;
1534         else if (rbw->objects == NULL)
1535                 rigidbody_update_ob_array(rbw);
1536
1537         /* advance simulation, we can only step one frame forward */
1538         if (ctime == rbw->ltime + 1) {
1539                 /* update and validate simulation */
1540                 rigidbody_update_simulation(scene, rbw, false);
1541
1542                 /* calculate how much time elapsed since last step in seconds */
1543                 timestep = 1.0f / (float)FPS * (ctime - rbw->ltime) * rbw->time_scale;
1544                 /* step simulation by the requested timestep, steps per second are adjusted to take time scale into account */
1545                 RB_dworld_step_simulation(rbw->physics_world, timestep, INT_MAX, 1.0f / (float)rbw->steps_per_second * min_ff(rbw->time_scale, 1.0f));
1546
1547                 rigidbody_update_simulation_post_step(rbw);
1548
1549                 rbw->ltime = ctime;
1550         }
1551 }
1552 /* ************************************** */
1553
1554 #else  /* WITH_BULLET */
1555
1556 /* stubs */
1557 #ifdef __GNUC__
1558 #  pragma GCC diagnostic push
1559 #  pragma GCC diagnostic ignored "-Wunused-parameter"
1560 #endif
1561
1562 struct RigidBodyOb *BKE_rigidbody_copy_object(Object *ob) { return NULL; }
1563 struct RigidBodyCon *BKE_rigidbody_copy_constraint(Object *ob) { return NULL; }
1564 void BKE_rigidbody_relink_constraint(RigidBodyCon *rbc) {}
1565 void BKE_rigidbody_validate_sim_world(Scene *scene, RigidBodyWorld *rbw, bool rebuild) {}
1566 void BKE_rigidbody_calc_volume(Object *ob, float *r_vol) { if (r_vol) *r_vol = 0.0f; }
1567 void BKE_rigidbody_calc_center_of_mass(Object *ob, float r_center[3]) { zero_v3(r_center); }
1568 struct RigidBodyWorld *BKE_rigidbody_create_world(Scene *scene) { return NULL; }
1569 struct RigidBodyWorld *BKE_rigidbody_world_copy(RigidBodyWorld *rbw) { return NULL; }
1570 void BKE_rigidbody_world_groups_relink(struct RigidBodyWorld *rbw) {}
1571 void BKE_rigidbody_world_id_loop(struct RigidBodyWorld *rbw, RigidbodyWorldIDFunc func, void *userdata) {}
1572 struct RigidBodyOb *BKE_rigidbody_create_object(Scene *scene, Object *ob, short type) { return NULL; }
1573 struct RigidBodyCon *BKE_rigidbody_create_constraint(Scene *scene, Object *ob, short type) { return NULL; }
1574 struct RigidBodyWorld *BKE_rigidbody_get_world(Scene *scene) { return NULL; }
1575 void BKE_rigidbody_remove_object(Scene *scene, Object *ob) {}
1576 void BKE_rigidbody_remove_constraint(Scene *scene, Object *ob) {}
1577 void BKE_rigidbody_sync_transforms(RigidBodyWorld *rbw, Object *ob, float ctime) {}
1578 void BKE_rigidbody_aftertrans_update(Object *ob, float loc[3], float rot[3], float quat[4], float rotAxis[3], float rotAngle) {}
1579 bool BKE_rigidbody_check_sim_running(RigidBodyWorld *rbw, float ctime) { return false; }
1580 void BKE_rigidbody_rebuild_world(Scene *scene, float ctime) {}
1581 void BKE_rigidbody_do_simulation(Scene *scene, float ctime) {}
1582
1583 #ifdef __GNUC__
1584 #  pragma GCC diagnostic pop
1585 #endif
1586
1587 #endif  /* WITH_BULLET */
1588
1589 /* -------------------- */
1590 /* Depsgraph evaluation */
1591
1592 void BKE_rigidbody_rebuild_sim(EvaluationContext *UNUSED(eval_ctx),
1593                                Scene *scene)
1594 {
1595         float ctime = BKE_scene_frame_get(scene);
1596
1597         if (G.debug & G_DEBUG_DEPSGRAPH) {
1598                 printf("%s at %f\n", __func__, ctime);
1599         }
1600
1601         /* rebuild sim data (i.e. after resetting to start of timeline) */
1602         if (BKE_scene_check_rigidbody_active(scene)) {
1603                 BKE_rigidbody_rebuild_world(scene, ctime);
1604         }
1605 }
1606
1607 void BKE_rigidbody_eval_simulation(EvaluationContext *UNUSED(eval_ctx),
1608                                    Scene *scene)
1609 {
1610         float ctime = BKE_scene_frame_get(scene);
1611
1612         if (G.debug & G_DEBUG_DEPSGRAPH) {
1613                 printf("%s at %f\n", __func__, ctime);
1614         }
1615
1616         /* evaluate rigidbody sim */
1617         if (BKE_scene_check_rigidbody_active(scene)) {
1618                 BKE_rigidbody_do_simulation(scene, ctime);
1619         }
1620 }
1621
1622 void BKE_rigidbody_object_sync_transforms(EvaluationContext *UNUSED(eval_ctx),
1623                                           Scene *scene,
1624                                           Object *ob)
1625 {
1626         RigidBodyWorld *rbw = scene->rigidbody_world;
1627         float ctime = BKE_scene_frame_get(scene);
1628
1629         if (G.debug & G_DEBUG_DEPSGRAPH) {
1630                 printf("%s on %s\n", __func__, ob->id.name);
1631         }
1632
1633         /* read values pushed into RBO from sim/cache... */
1634         BKE_rigidbody_sync_transforms(rbw, ob, ctime);
1635 }