1 #ifdef HAVE_CONFIG_H
2 # include <config.h>
3 #endif
4
5 #include "ephysics_private.h"
6
7 #ifdef __cplusplus
8 extern "C" {
9 #endif
10
11 struct _EPhysics_Constraint {
12 btGeneric6DofConstraint *bt_constraint;
13 EPhysics_World *world;
14 EPhysics_Body *bodies[2];
15 };
16
17 static void
_ephysics_constraint_del(EPhysics_Constraint * constraint)18 _ephysics_constraint_del(EPhysics_Constraint *constraint)
19 {
20 ephysics_world_constraint_del(constraint->world, constraint,
21 constraint->bt_constraint);
22 delete constraint->bt_constraint;
23 free(constraint);
24 }
25
26 void
ephysics_constraint_body_del(EPhysics_Body * body)27 ephysics_constraint_body_del(EPhysics_Body *body)
28 {
29 void *ldata;
30 EPhysics_Constraint *constraint;
31 Eina_List *l, *constraints, *rem = NULL;
32
33 constraints = ephysics_world_constraints_get(body->world);
34 if (!constraints) return;
35
36 EINA_LIST_FOREACH(constraints, l, ldata)
37 {
38 constraint = (EPhysics_Constraint *)ldata;
39 if (constraint->bodies[0] == body || constraint->bodies[1])
40 rem = eina_list_append(rem, constraint);
41 }
42
43 EINA_LIST_FREE(rem, ldata)
44 _ephysics_constraint_del((EPhysics_Constraint *)ldata);
45 }
46
47 static void
_ephysics_constraint_linear_limit_get(const EPhysics_Constraint * constraint,Evas_Coord * lower_x,Evas_Coord * upper_x,Evas_Coord * lower_y,Evas_Coord * upper_y,Evas_Coord * lower_z,Evas_Coord * upper_z,double rate)48 _ephysics_constraint_linear_limit_get(const EPhysics_Constraint *constraint, Evas_Coord *lower_x, Evas_Coord *upper_x, Evas_Coord *lower_y, Evas_Coord *upper_y, Evas_Coord *lower_z, Evas_Coord *upper_z, double rate)
49 {
50 btVector3 linear_limit;
51
52 if (lower_x || lower_y || lower_z)
53 {
54 constraint->bt_constraint->getLinearLowerLimit(linear_limit);
55
56 if (lower_x) *lower_x = linear_limit.getX() * rate;
57 if (lower_y) *lower_y = linear_limit.getY() * rate;
58 if (lower_z) *lower_z = linear_limit.getZ() * rate;
59 }
60
61 if (upper_x || upper_y || upper_z)
62 {
63 constraint->bt_constraint->getLinearUpperLimit(linear_limit);
64
65 if (upper_x) *upper_x = linear_limit.getX() * rate;
66 if (upper_y) *upper_y = linear_limit.getY() * rate;
67 if (upper_z) *upper_z = linear_limit.getZ() * rate;
68 }
69 }
70
71 static void
_ephysics_constraint_linear_limit_set(EPhysics_Constraint * constraint,Evas_Coord lower_x,Evas_Coord upper_x,Evas_Coord lower_y,Evas_Coord upper_y,Evas_Coord lower_z,Evas_Coord upper_z,double rate)72 _ephysics_constraint_linear_limit_set(EPhysics_Constraint *constraint, Evas_Coord lower_x, Evas_Coord upper_x, Evas_Coord lower_y, Evas_Coord upper_y, Evas_Coord lower_z, Evas_Coord upper_z, double rate)
73 {
74 lower_x = (lower_x) / rate;
75 upper_x = (upper_x) / rate;
76
77 lower_y = (lower_y) / rate;
78 upper_y = (upper_y) / rate;
79
80 lower_z = (lower_z) / rate;
81 upper_z = (upper_z) / rate;
82
83 constraint->bt_constraint->setLinearLowerLimit(btVector3(-lower_x, -upper_y,
84 -lower_z));
85 constraint->bt_constraint->setLinearUpperLimit(btVector3(upper_x, upper_y,
86 upper_z));
87 }
88
89 void
ephysics_constraint_recalc(EPhysics_Constraint * constraint,double rate)90 ephysics_constraint_recalc(EPhysics_Constraint *constraint, double rate)
91 {
92 Evas_Coord lower_x, upper_x, lower_y, upper_y, lower_z, upper_z;
93
94 _ephysics_constraint_linear_limit_get(constraint, &lower_x, &upper_x,
95 &lower_y, &upper_y, &lower_z, &upper_z, rate);
96 _ephysics_constraint_linear_limit_set(constraint, lower_x, upper_x, lower_y,
97 upper_y, lower_z, upper_z, rate);
98 }
99
100 EAPI EPhysics_Constraint *
ephysics_constraint_add(EPhysics_Body * body)101 ephysics_constraint_add(EPhysics_Body *body)
102 {
103 EPhysics_Constraint *constraint;
104 btTransform trans;
105
106 if (!body)
107 {
108 ERR("To create a constraint body must to be non null.");
109 return NULL;
110 }
111
112 constraint = (EPhysics_Constraint *) calloc(1, sizeof(EPhysics_Constraint));
113 if (!constraint)
114 {
115 ERR("Couldn't create a new constraint.");
116 return NULL;
117 }
118
119 ephysics_world_lock_take(ephysics_body_world_get(body));
120 trans.setIdentity();
121 trans.setOrigin(btVector3(0, 0, 0));
122 constraint->bt_constraint = new
123 btGeneric6DofConstraint(*ephysics_body_rigid_body_get(body), trans,
124 false);
125
126 if (!constraint->bt_constraint)
127 {
128 ERR("Failed to create a btConstraint");
129 free(constraint);
130 return NULL;
131 }
132
133 constraint->bodies[0] = body;
134 constraint->world = ephysics_body_world_get(body);
135 ephysics_world_constraint_add(constraint->world, constraint,
136 constraint->bt_constraint);
137
138 INF("Constraint added.");
139 ephysics_world_lock_release(ephysics_body_world_get(body));
140 return constraint;
141 }
142
143 EAPI void
ephysics_constraint_linear_limit_set(EPhysics_Constraint * constraint,Evas_Coord lower_x,Evas_Coord upper_x,Evas_Coord lower_y,Evas_Coord upper_y,Evas_Coord lower_z,Evas_Coord upper_z)144 ephysics_constraint_linear_limit_set(EPhysics_Constraint *constraint, Evas_Coord lower_x, Evas_Coord upper_x, Evas_Coord lower_y, Evas_Coord upper_y, Evas_Coord lower_z, Evas_Coord upper_z)
145 {
146 double rate;
147
148 if (!constraint)
149 {
150 ERR("Can't set constraint's linear limit, constraint is null.");
151 return;
152 }
153
154 ephysics_world_lock_take(constraint->world);
155 rate = ephysics_world_rate_get(constraint->world);
156 _ephysics_constraint_linear_limit_set(constraint, lower_x, upper_x, lower_y,
157 upper_y, lower_z, upper_z, rate);
158 ephysics_world_lock_release(constraint->world);
159 }
160
161 EAPI void
ephysics_constraint_linear_limit_get(const EPhysics_Constraint * constraint,Evas_Coord * lower_x,Evas_Coord * upper_x,Evas_Coord * lower_y,Evas_Coord * upper_y,Evas_Coord * lower_z,Evas_Coord * upper_z)162 ephysics_constraint_linear_limit_get(const EPhysics_Constraint *constraint, Evas_Coord *lower_x, Evas_Coord *upper_x, Evas_Coord *lower_y, Evas_Coord *upper_y, Evas_Coord *lower_z, Evas_Coord *upper_z)
163 {
164 int rate;
165
166 if (!constraint)
167 {
168 ERR("Can't get constraint's linear limit, constraint is null.");
169 return;
170 }
171
172 rate = ephysics_world_rate_get(constraint->world);
173 _ephysics_constraint_linear_limit_get(constraint, lower_x, upper_x, lower_y,
174 upper_y, lower_z, upper_z, rate);
175 }
176
177 EAPI void
ephysics_constraint_angular_limit_set(EPhysics_Constraint * constraint,double counter_clock_x,double clock_wise_x,double counter_clock_y,double clock_wise_y,double counter_clock_z,double clock_wise_z)178 ephysics_constraint_angular_limit_set(EPhysics_Constraint *constraint, double counter_clock_x, double clock_wise_x, double counter_clock_y, double clock_wise_y, double counter_clock_z, double clock_wise_z)
179 {
180
181 if (!constraint)
182 {
183 ERR("Can't set constraint's angular limit, constraint is null.");
184 return;
185 }
186
187 ephysics_world_lock_take(constraint->world);
188
189 constraint->bt_constraint->setAngularLowerLimit(btVector3(
190 -counter_clock_x / RAD_TO_DEG, -counter_clock_y / RAD_TO_DEG,
191 -counter_clock_z/RAD_TO_DEG));
192 constraint->bt_constraint->setAngularUpperLimit(btVector3(
193 clock_wise_x / RAD_TO_DEG, clock_wise_y / RAD_TO_DEG,
194 clock_wise_z/RAD_TO_DEG));
195 ephysics_world_lock_release(constraint->world);
196 }
197
198 EAPI void
ephysics_constraint_angular_limit_get(const EPhysics_Constraint * constraint,double * counter_clock_x,double * clock_wise_x,double * counter_clock_y,double * clock_wise_y,double * counter_clock_z,double * clock_wise_z)199 ephysics_constraint_angular_limit_get(const EPhysics_Constraint *constraint, double *counter_clock_x, double *clock_wise_x, double *counter_clock_y, double *clock_wise_y, double *counter_clock_z, double *clock_wise_z)
200 {
201 btVector3 angular_limit;
202
203 if (!constraint)
204 {
205 ERR("Can't get constraint's angular limit, constraint is null.");
206 return;
207 }
208
209 if (counter_clock_x)
210 {
211 constraint->bt_constraint->getAngularLowerLimit(angular_limit);
212 *counter_clock_x = angular_limit.getX() * RAD_TO_DEG;
213 }
214
215 if (clock_wise_x)
216 {
217 constraint->bt_constraint->getAngularUpperLimit(angular_limit);
218 *clock_wise_x = angular_limit.getX() * RAD_TO_DEG;
219 }
220
221 if (counter_clock_y)
222 {
223 constraint->bt_constraint->getAngularLowerLimit(angular_limit);
224 *counter_clock_y = angular_limit.getY() * RAD_TO_DEG;
225 }
226
227 if (clock_wise_y)
228 {
229 constraint->bt_constraint->getAngularUpperLimit(angular_limit);
230 *clock_wise_y = angular_limit.getY() * RAD_TO_DEG;
231 }
232
233 if (counter_clock_z)
234 {
235 constraint->bt_constraint->getAngularLowerLimit(angular_limit);
236 *counter_clock_z = angular_limit.getZ() * RAD_TO_DEG;
237 }
238
239 if (clock_wise_z)
240 {
241 constraint->bt_constraint->getAngularUpperLimit(angular_limit);
242 *clock_wise_z = angular_limit.getZ() * RAD_TO_DEG;
243 }
244 }
245
246 EAPI void
ephysics_constraint_anchor_set(EPhysics_Constraint * constraint,Evas_Coord anchor_b1_x,Evas_Coord anchor_b1_y,Evas_Coord anchor_b1_z,Evas_Coord anchor_b2_x,Evas_Coord anchor_b2_y,Evas_Coord anchor_b2_z)247 ephysics_constraint_anchor_set(EPhysics_Constraint *constraint, Evas_Coord anchor_b1_x, Evas_Coord anchor_b1_y, Evas_Coord anchor_b1_z, Evas_Coord anchor_b2_x, Evas_Coord anchor_b2_y, Evas_Coord anchor_b2_z)
248 {
249 btTransform anchor_b1;
250 btTransform anchor_b2;
251 btTransform center_mass;
252 double rate;
253 Evas_Coord b1x, b1y, b1z, b1w, b1h, b1d, b2x, b2y, b2z, b2w, b2h, b2d, wx, wy,
254 wh;
255 btScalar ab1x, ab1y, ab1z, ab2x, ab2y, ab2z;
256
257 if (!constraint)
258 {
259 ERR("Can't set constraint's anchors, constraint is null.");
260 return;
261 }
262
263 ephysics_world_lock_take(constraint->world);
264
265 ephysics_world_render_geometry_get(constraint->world, &wx, &wy, NULL, NULL,
266 &wh, NULL);
267
268 ephysics_body_geometry_get(constraint->bodies[0], &b1x, &b1y, &b1z, &b1w,
269 &b1h, &b1d);
270
271 rate = ephysics_world_rate_get(constraint->world);
272
273 ab1x = (anchor_b1_x - (b1x + b1w / 2)) / rate;
274 ab1y = (anchor_b1_y - (b1y + b1h / 2)) / rate;
275 ab1z = (anchor_b1_z - (b1z + b1d / 2)) / rate;
276 DBG("body1 anchor set to: %lf, %lf, %lf", ab1x, ab1y, ab1z);
277
278 anchor_b1.setIdentity();
279 anchor_b1.setOrigin(btVector3(ab1x, ab1y, ab1z));
280
281 if (constraint->bodies[1])
282 {
283 ephysics_body_geometry_get(constraint->bodies[1], &b2x, &b2y, &b2z, &b2w,
284 &b2h, &b2d);
285
286 ab2x = (anchor_b2_x - (b2x + b2w / 2)) / rate;
287 ab2y = (anchor_b2_y - (b2y + b2h / 2)) / rate;
288 ab2z = (anchor_b2_z - (b2z + b2d / 2)) / rate;
289
290 DBG("body2 anchor set to: %lf, %lf, %lf", ab2x, ab2y, ab2z);
291
292 anchor_b2.setIdentity();
293 anchor_b2.setOrigin(btVector3(ab2x, ab2y, ab2z));
294 }
295 else
296 {
297 anchor_b2.setIdentity();
298 anchor_b2.setOrigin(btVector3(anchor_b1.getOrigin().x(),
299 anchor_b1.getOrigin().y(),
300 anchor_b1.getOrigin().z()));
301
302 center_mass = constraint->bodies[0]->rigid_body->
303 getCenterOfMassTransform();
304
305 anchor_b1.setIdentity();
306 anchor_b1 = center_mass * anchor_b2;
307 }
308
309 constraint->bt_constraint->setFrames(anchor_b1, anchor_b2);
310 ephysics_world_lock_release(constraint->world);
311 }
312
313 EAPI void
ephysics_constraint_anchor_get(const EPhysics_Constraint * constraint,Evas_Coord * anchor_b1_x,Evas_Coord * anchor_b1_y,Evas_Coord * anchor_b1_z,Evas_Coord * anchor_b2_x,Evas_Coord * anchor_b2_y,Evas_Coord * anchor_b2_z)314 ephysics_constraint_anchor_get(const EPhysics_Constraint *constraint, Evas_Coord *anchor_b1_x, Evas_Coord *anchor_b1_y, Evas_Coord *anchor_b1_z, Evas_Coord *anchor_b2_x, Evas_Coord *anchor_b2_y, Evas_Coord *anchor_b2_z)
315 {
316 btTransform anchor_b1;
317 btTransform anchor_b2;
318 double rate;
319
320 if (!constraint)
321 {
322 ERR("Can't set constraint's anchors, constraint is null.");
323 return;
324 }
325
326 rate = ephysics_world_rate_get(constraint->world);
327
328 anchor_b1 = constraint->bt_constraint->getFrameOffsetA();
329 anchor_b2 = constraint->bt_constraint->getFrameOffsetB();
330
331 if (anchor_b1_x) *anchor_b1_x = round(anchor_b1.getOrigin().x() * rate);
332 if (anchor_b1_y) *anchor_b1_y = round(anchor_b1.getOrigin().y() * rate);
333 if (anchor_b1_z) *anchor_b1_z = round(anchor_b1.getOrigin().z() * rate);
334 if (anchor_b2_x) *anchor_b2_x = round(anchor_b2.getOrigin().x() * rate);
335 if (anchor_b2_y) *anchor_b2_y = round(anchor_b2.getOrigin().y() * rate);
336 if (anchor_b2_z) *anchor_b2_z = round(anchor_b2.getOrigin().z() * rate);
337 }
338
339 EAPI EPhysics_Constraint *
ephysics_constraint_linked_add(EPhysics_Body * body1,EPhysics_Body * body2)340 ephysics_constraint_linked_add(EPhysics_Body *body1, EPhysics_Body *body2)
341 {
342 EPhysics_Constraint *constraint;
343
344 if (!body1 || !body2)
345 {
346 ERR("To create a linked constraint body1 and bod2 must to be non null.");
347 return NULL;
348 }
349
350 if (ephysics_body_world_get(body1) != ephysics_body_world_get(body2))
351 {
352 ERR("To create a constraint both bodies must belong to the same"
353 "world.");
354 return NULL;
355 }
356
357 if (body1->type == EPHYSICS_BODY_TYPE_CLOTH ||
358 body2->type == EPHYSICS_BODY_TYPE_CLOTH)
359 {
360 ERR("Constraints are allowed only between rigid -> rigid bodies or"
361 "rigid -> soft bodies");
362 return NULL;
363 }
364
365 constraint = (EPhysics_Constraint *) calloc(1, sizeof(EPhysics_Constraint));
366 if (!constraint)
367 {
368 ERR("Couldn't create a new constraint.");
369 return NULL;
370 }
371
372 constraint->world = ephysics_body_world_get(body1);
373 constraint->bodies[0] = body1;
374 constraint->bodies[1] = body2;
375
376 ephysics_world_lock_take(constraint->world);
377
378 constraint->bt_constraint = new btGeneric6DofConstraint(
379 *ephysics_body_rigid_body_get(body1), *ephysics_body_rigid_body_get(body2),
380 btTransform(), btTransform(), false);
381
382 if (!constraint->bt_constraint)
383 {
384 ephysics_world_lock_release(constraint->world);
385 free(constraint);
386 return NULL;
387 }
388
389 ephysics_world_constraint_add(constraint->world, constraint,
390 constraint->bt_constraint);
391
392 ephysics_world_lock_release(constraint->world);
393 INF("Constraint added.");
394 return constraint;
395 }
396
397 EAPI void
ephysics_constraint_del(EPhysics_Constraint * constraint)398 ephysics_constraint_del(EPhysics_Constraint *constraint)
399 {
400 if (!constraint)
401 {
402 ERR("Can't delete constraint, it wasn't provided.");
403 return;
404 }
405
406 // technically at this point locking and unlocking is pointless because
407 // if another thread is accessing this constraint, after this point it
408 // will be broken locks or not. this removes a segv in locking something
409 // we freed in _ephysics_constraint_del() by not locking.
410 _ephysics_constraint_del(constraint);
411 INF("Constraint deleted.");
412 }
413
414
415 #ifdef __cplusplus
416 }
417 #endif
418