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