1 /*
2 * This program is free software; you can redistribute it and/or
3 * modify it under the terms of the GNU General Public License
4 * as published by the Free Software Foundation; either version 2
5 * of the License, or (at your option) any later version.
6 *
7 * This program is distributed in the hope that it will be useful,
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 * GNU General Public License for more details.
11 *
12 * You should have received a copy of the GNU General Public License
13 * along with this program; if not, write to the Free Software Foundation,
14 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15 *
16 * The Original Code is Copyright (C) 2009 by Janne Karhu.
17 * All rights reserved.
18 */
19
20 /** \file
21 * \ingroup bke
22 */
23
24 #include <math.h>
25 #include <string.h>
26
27 #include "MEM_guardedalloc.h"
28
29 #include "DNA_object_force_types.h"
30 #include "DNA_scene_types.h"
31
32 #include "BLI_blenlib.h"
33 #include "BLI_kdtree.h"
34 #include "BLI_math.h"
35 #include "BLI_rand.h"
36 #include "BLI_utildefines.h"
37
38 #include "BKE_boids.h"
39 #include "BKE_collision.h"
40 #include "BKE_effect.h"
41 #include "BKE_particle.h"
42
43 #include "BKE_modifier.h"
44
45 #include "RNA_enum_types.h"
46
len_squared_v3v3_with_normal_bias(const float co_search[3],const float co_test[3],const void * user_data)47 static float len_squared_v3v3_with_normal_bias(const float co_search[3],
48 const float co_test[3],
49 const void *user_data)
50 {
51 const float *normal = user_data;
52 float d[3], dist;
53
54 sub_v3_v3v3(d, co_test, co_search);
55
56 dist = len_squared_v3(d);
57
58 /* Avoid head-on collisions. */
59 if (dot_v3v3(d, normal) < 0.0f) {
60 dist *= 10.0f;
61 }
62 return dist;
63 }
64
65 typedef struct BoidValues {
66 float max_speed, max_acc;
67 float max_ave, min_speed;
68 float personal_space, jump_speed;
69 } BoidValues;
70
71 static bool apply_boid_rule(
72 BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness);
73
rule_none(BoidRule * UNUSED (rule),BoidBrainData * UNUSED (data),BoidValues * UNUSED (val),ParticleData * UNUSED (pa))74 static bool rule_none(BoidRule *UNUSED(rule),
75 BoidBrainData *UNUSED(data),
76 BoidValues *UNUSED(val),
77 ParticleData *UNUSED(pa))
78 {
79 return false;
80 }
81
rule_goal_avoid(BoidRule * rule,BoidBrainData * bbd,BoidValues * val,ParticleData * pa)82 static bool rule_goal_avoid(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa)
83 {
84 BoidRuleGoalAvoid *gabr = (BoidRuleGoalAvoid *)rule;
85 BoidSettings *boids = bbd->part->boids;
86 BoidParticle *bpa = pa->boid;
87 EffectedPoint epoint;
88 ListBase *effectors = bbd->sim->psys->effectors;
89 EffectorCache *cur, *eff = NULL;
90 EffectorCache temp_eff;
91 EffectorData efd, cur_efd;
92 float mul = (rule->type == eBoidRuleType_Avoid ? 1.0 : -1.0);
93 float priority = 0.0f, len = 0.0f;
94 bool ret = false;
95
96 int p = 0;
97 efd.index = cur_efd.index = &p;
98
99 pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint);
100
101 /* first find out goal/predator with highest priority */
102 if (effectors) {
103 for (cur = effectors->first; cur; cur = cur->next) {
104 Object *eob = cur->ob;
105 PartDeflect *pd = cur->pd;
106
107 if (gabr->ob && (rule->type != eBoidRuleType_Goal || gabr->ob != bpa->ground)) {
108 if (gabr->ob == eob) {
109 /* TODO: effectors with multiple points */
110 if (get_effector_data(cur, &efd, &epoint, 0)) {
111 if (cur->pd && cur->pd->forcefield == PFIELD_BOID) {
112 priority = mul * pd->f_strength *
113 effector_falloff(cur, &efd, &epoint, bbd->part->effector_weights);
114 }
115 else {
116 priority = 1.0;
117 }
118
119 eff = cur;
120 }
121 break;
122 }
123 }
124 else if (rule->type == eBoidRuleType_Goal && eob == bpa->ground) {
125 /* skip current object */
126 }
127 else if (pd->forcefield == PFIELD_BOID && mul * pd->f_strength > 0.0f &&
128 get_effector_data(cur, &cur_efd, &epoint, 0)) {
129 float temp = mul * pd->f_strength *
130 effector_falloff(cur, &cur_efd, &epoint, bbd->part->effector_weights);
131
132 if (temp == 0.0f) {
133 /* do nothing */
134 }
135 else if (temp > priority) {
136 priority = temp;
137 eff = cur;
138 efd = cur_efd;
139 len = efd.distance;
140 }
141 /* choose closest object with same priority */
142 else if (temp == priority && efd.distance < len) {
143 eff = cur;
144 efd = cur_efd;
145 len = efd.distance;
146 }
147 }
148 }
149 }
150
151 /* if the object doesn't have effector data we have to fake it */
152 if (eff == NULL && gabr->ob) {
153 memset(&temp_eff, 0, sizeof(EffectorCache));
154 temp_eff.ob = gabr->ob;
155 temp_eff.depsgraph = bbd->sim->depsgraph;
156 temp_eff.scene = bbd->sim->scene;
157 eff = &temp_eff;
158 get_effector_data(eff, &efd, &epoint, 0);
159 priority = 1.0f;
160 }
161
162 /* then use that effector */
163 if (priority > (rule->type == eBoidRuleType_Avoid ?
164 gabr->fear_factor :
165 0.0f)) { /* with avoid, factor is "fear factor" */
166 Object *eob = eff->ob;
167 PartDeflect *pd = eff->pd;
168 float surface = (pd && pd->shape == PFIELD_SHAPE_SURFACE) ? 1.0f : 0.0f;
169
170 if (gabr->options & BRULE_GOAL_AVOID_PREDICT) {
171 /* estimate future location of target */
172 get_effector_data(eff, &efd, &epoint, 1);
173
174 mul_v3_fl(efd.vel, efd.distance / (val->max_speed * bbd->timestep));
175 add_v3_v3(efd.loc, efd.vel);
176 sub_v3_v3v3(efd.vec_to_point, pa->prev_state.co, efd.loc);
177 efd.distance = len_v3(efd.vec_to_point);
178 }
179
180 if (rule->type == eBoidRuleType_Goal && boids->options & BOID_ALLOW_CLIMB && surface != 0.0f) {
181 if (!bbd->goal_ob || bbd->goal_priority < priority) {
182 bbd->goal_ob = eob;
183 copy_v3_v3(bbd->goal_co, efd.loc);
184 copy_v3_v3(bbd->goal_nor, efd.nor);
185 }
186 }
187 else if ((rule->type == eBoidRuleType_Avoid) && (bpa->data.mode == eBoidMode_Climbing) &&
188 (priority > 2.0f * gabr->fear_factor)) {
189 /* detach from surface and try to fly away from danger */
190 negate_v3_v3(efd.vec_to_point, bpa->gravity);
191 }
192
193 copy_v3_v3(bbd->wanted_co, efd.vec_to_point);
194 mul_v3_fl(bbd->wanted_co, mul);
195
196 bbd->wanted_speed = val->max_speed * priority;
197
198 /* with goals factor is approach velocity factor */
199 if (rule->type == eBoidRuleType_Goal && boids->landing_smoothness > 0.0f) {
200 float len2 = 2.0f * len_v3(pa->prev_state.vel);
201
202 surface *= pa->size * boids->height;
203
204 if (len2 > 0.0f && efd.distance - surface < len2) {
205 len2 = (efd.distance - surface) / len2;
206 bbd->wanted_speed *= powf(len2, boids->landing_smoothness);
207 }
208 }
209
210 ret = true;
211 }
212
213 return ret;
214 }
215
rule_avoid_collision(BoidRule * rule,BoidBrainData * bbd,BoidValues * val,ParticleData * pa)216 static bool rule_avoid_collision(BoidRule *rule,
217 BoidBrainData *bbd,
218 BoidValues *val,
219 ParticleData *pa)
220 {
221 const int raycast_flag = BVH_RAYCAST_DEFAULT & ~BVH_RAYCAST_WATERTIGHT;
222 BoidRuleAvoidCollision *acbr = (BoidRuleAvoidCollision *)rule;
223 KDTreeNearest_3d *ptn = NULL;
224 ParticleTarget *pt;
225 BoidParticle *bpa = pa->boid;
226 ColliderCache *coll;
227 float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f};
228 float co1[3], vel1[3], co2[3], vel2[3];
229 float len, t, inp, t_min = 2.0f;
230 int n, neighbors = 0, nearest = 0;
231 bool ret = 0;
232
233 // check deflector objects first
234 if (acbr->options & BRULE_ACOLL_WITH_DEFLECTORS && bbd->sim->colliders) {
235 ParticleCollision col;
236 BVHTreeRayHit hit;
237 float radius = val->personal_space * pa->size, ray_dir[3];
238
239 memset(&col, 0, sizeof(ParticleCollision));
240
241 copy_v3_v3(col.co1, pa->prev_state.co);
242 add_v3_v3v3(col.co2, pa->prev_state.co, pa->prev_state.vel);
243 sub_v3_v3v3(ray_dir, col.co2, col.co1);
244 mul_v3_fl(ray_dir, acbr->look_ahead);
245 col.f = 0.0f;
246 hit.index = -1;
247 hit.dist = col.original_ray_length = normalize_v3(ray_dir);
248
249 /* find out closest deflector object */
250 for (coll = bbd->sim->colliders->first; coll; coll = coll->next) {
251 /* don't check with current ground object */
252 if (coll->ob == bpa->ground) {
253 continue;
254 }
255
256 col.current = coll->ob;
257 col.md = coll->collmd;
258
259 if (col.md && col.md->bvhtree) {
260 BLI_bvhtree_ray_cast_ex(col.md->bvhtree,
261 col.co1,
262 ray_dir,
263 radius,
264 &hit,
265 BKE_psys_collision_neartest_cb,
266 &col,
267 raycast_flag);
268 }
269 }
270 /* then avoid that object */
271 if (hit.index >= 0) {
272 t = hit.dist / col.original_ray_length;
273
274 /* avoid head-on collision */
275 if (dot_v3v3(col.pce.nor, pa->prev_state.ave) < -0.99f) {
276 /* don't know why, but uneven range [0.0, 1.0] */
277 /* works much better than even [-1.0, 1.0] */
278 bbd->wanted_co[0] = BLI_rng_get_float(bbd->rng);
279 bbd->wanted_co[1] = BLI_rng_get_float(bbd->rng);
280 bbd->wanted_co[2] = BLI_rng_get_float(bbd->rng);
281 }
282 else {
283 copy_v3_v3(bbd->wanted_co, col.pce.nor);
284 }
285
286 mul_v3_fl(bbd->wanted_co, (1.0f - t) * val->personal_space * pa->size);
287
288 bbd->wanted_speed = sqrtf(t) * len_v3(pa->prev_state.vel);
289 bbd->wanted_speed = MAX2(bbd->wanted_speed, val->min_speed);
290
291 return true;
292 }
293 }
294
295 // check boids in own system
296 if (acbr->options & BRULE_ACOLL_WITH_BOIDS) {
297 neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb(bbd->sim->psys->tree,
298 pa->prev_state.co,
299 &ptn,
300 acbr->look_ahead *
301 len_v3(pa->prev_state.vel),
302 len_squared_v3v3_with_normal_bias,
303 pa->prev_state.ave);
304 if (neighbors > 1) {
305 for (n = 1; n < neighbors; n++) {
306 copy_v3_v3(co1, pa->prev_state.co);
307 copy_v3_v3(vel1, pa->prev_state.vel);
308 copy_v3_v3(co2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.co);
309 copy_v3_v3(vel2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.vel);
310
311 sub_v3_v3v3(loc, co1, co2);
312
313 sub_v3_v3v3(vec, vel1, vel2);
314
315 inp = dot_v3v3(vec, vec);
316
317 /* velocities not parallel */
318 if (inp != 0.0f) {
319 t = -dot_v3v3(loc, vec) / inp;
320 /* cpa is not too far in the future so investigate further */
321 if (t > 0.0f && t < t_min) {
322 madd_v3_v3fl(co1, vel1, t);
323 madd_v3_v3fl(co2, vel2, t);
324
325 sub_v3_v3v3(vec, co2, co1);
326
327 len = normalize_v3(vec);
328
329 /* distance of cpa is close enough */
330 if (len < 2.0f * val->personal_space * pa->size) {
331 t_min = t;
332
333 mul_v3_fl(vec, len_v3(vel1));
334 mul_v3_fl(vec, (2.0f - t) / 2.0f);
335 sub_v3_v3v3(bbd->wanted_co, vel1, vec);
336 bbd->wanted_speed = len_v3(bbd->wanted_co);
337 ret = 1;
338 }
339 }
340 }
341 }
342 }
343 }
344 if (ptn) {
345 MEM_freeN(ptn);
346 ptn = NULL;
347 }
348
349 /* check boids in other systems */
350 for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) {
351 ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt);
352
353 if (epsys) {
354 BLI_assert(epsys->tree != NULL);
355 neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb(epsys->tree,
356 pa->prev_state.co,
357 &ptn,
358 acbr->look_ahead *
359 len_v3(pa->prev_state.vel),
360 len_squared_v3v3_with_normal_bias,
361 pa->prev_state.ave);
362
363 if (neighbors > 0) {
364 for (n = 0; n < neighbors; n++) {
365 copy_v3_v3(co1, pa->prev_state.co);
366 copy_v3_v3(vel1, pa->prev_state.vel);
367 copy_v3_v3(co2, (epsys->particles + ptn[n].index)->prev_state.co);
368 copy_v3_v3(vel2, (epsys->particles + ptn[n].index)->prev_state.vel);
369
370 sub_v3_v3v3(loc, co1, co2);
371
372 sub_v3_v3v3(vec, vel1, vel2);
373
374 inp = dot_v3v3(vec, vec);
375
376 /* velocities not parallel */
377 if (inp != 0.0f) {
378 t = -dot_v3v3(loc, vec) / inp;
379 /* cpa is not too far in the future so investigate further */
380 if (t > 0.0f && t < t_min) {
381 madd_v3_v3fl(co1, vel1, t);
382 madd_v3_v3fl(co2, vel2, t);
383
384 sub_v3_v3v3(vec, co2, co1);
385
386 len = normalize_v3(vec);
387
388 /* distance of cpa is close enough */
389 if (len < 2.0f * val->personal_space * pa->size) {
390 t_min = t;
391
392 mul_v3_fl(vec, len_v3(vel1));
393 mul_v3_fl(vec, (2.0f - t) / 2.0f);
394 sub_v3_v3v3(bbd->wanted_co, vel1, vec);
395 bbd->wanted_speed = len_v3(bbd->wanted_co);
396 ret = 1;
397 }
398 }
399 }
400 }
401 }
402
403 if (ptn) {
404 MEM_freeN(ptn);
405 ptn = NULL;
406 }
407 }
408 }
409
410 if (ptn && nearest == 0) {
411 MEM_freeN(ptn);
412 }
413
414 return ret;
415 }
rule_separate(BoidRule * UNUSED (rule),BoidBrainData * bbd,BoidValues * val,ParticleData * pa)416 static bool rule_separate(BoidRule *UNUSED(rule),
417 BoidBrainData *bbd,
418 BoidValues *val,
419 ParticleData *pa)
420 {
421 KDTreeNearest_3d *ptn = NULL;
422 ParticleTarget *pt;
423 float len = 2.0f * val->personal_space * pa->size + 1.0f;
424 float vec[3] = {0.0f, 0.0f, 0.0f};
425 int neighbors = BLI_kdtree_3d_range_search(
426 bbd->sim->psys->tree, pa->prev_state.co, &ptn, 2.0f * val->personal_space * pa->size);
427 bool ret = false;
428
429 if (neighbors > 1 && ptn[1].dist != 0.0f) {
430 sub_v3_v3v3(vec, pa->prev_state.co, bbd->sim->psys->particles[ptn[1].index].state.co);
431 mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[1].dist) / ptn[1].dist);
432 add_v3_v3(bbd->wanted_co, vec);
433 bbd->wanted_speed = val->max_speed;
434 len = ptn[1].dist;
435 ret = 1;
436 }
437 if (ptn) {
438 MEM_freeN(ptn);
439 ptn = NULL;
440 }
441
442 /* check other boid systems */
443 for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) {
444 ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt);
445
446 if (epsys) {
447 neighbors = BLI_kdtree_3d_range_search(
448 epsys->tree, pa->prev_state.co, &ptn, 2.0f * val->personal_space * pa->size);
449
450 if (neighbors > 0 && ptn[0].dist < len) {
451 sub_v3_v3v3(vec, pa->prev_state.co, ptn[0].co);
452 mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[0].dist) / ptn[1].dist);
453 add_v3_v3(bbd->wanted_co, vec);
454 bbd->wanted_speed = val->max_speed;
455 len = ptn[0].dist;
456 ret = true;
457 }
458
459 if (ptn) {
460 MEM_freeN(ptn);
461 ptn = NULL;
462 }
463 }
464 }
465 return ret;
466 }
rule_flock(BoidRule * UNUSED (rule),BoidBrainData * bbd,BoidValues * UNUSED (val),ParticleData * pa)467 static bool rule_flock(BoidRule *UNUSED(rule),
468 BoidBrainData *bbd,
469 BoidValues *UNUSED(val),
470 ParticleData *pa)
471 {
472 KDTreeNearest_3d ptn[11];
473 float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f};
474 int neighbors = BLI_kdtree_3d_find_nearest_n_with_len_squared_cb(
475 bbd->sim->psys->tree,
476 pa->state.co,
477 ptn,
478 ARRAY_SIZE(ptn),
479 len_squared_v3v3_with_normal_bias,
480 pa->prev_state.ave);
481 int n;
482 bool ret = false;
483
484 if (neighbors > 1) {
485 for (n = 1; n < neighbors; n++) {
486 add_v3_v3(loc, bbd->sim->psys->particles[ptn[n].index].prev_state.co);
487 add_v3_v3(vec, bbd->sim->psys->particles[ptn[n].index].prev_state.vel);
488 }
489
490 mul_v3_fl(loc, 1.0f / ((float)neighbors - 1.0f));
491 mul_v3_fl(vec, 1.0f / ((float)neighbors - 1.0f));
492
493 sub_v3_v3(loc, pa->prev_state.co);
494 sub_v3_v3(vec, pa->prev_state.vel);
495
496 add_v3_v3(bbd->wanted_co, vec);
497 add_v3_v3(bbd->wanted_co, loc);
498 bbd->wanted_speed = len_v3(bbd->wanted_co);
499
500 ret = true;
501 }
502 return ret;
503 }
rule_follow_leader(BoidRule * rule,BoidBrainData * bbd,BoidValues * val,ParticleData * pa)504 static bool rule_follow_leader(BoidRule *rule,
505 BoidBrainData *bbd,
506 BoidValues *val,
507 ParticleData *pa)
508 {
509 BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader *)rule;
510 float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f};
511 float mul, len;
512 int n = (flbr->queue_size <= 1) ? bbd->sim->psys->totpart : flbr->queue_size;
513 int i, p = pa - bbd->sim->psys->particles;
514 bool ret = false;
515
516 if (flbr->ob) {
517 float vec2[3], t;
518
519 /* first check we're not blocking the leader */
520 sub_v3_v3v3(vec, flbr->loc, flbr->oloc);
521 mul_v3_fl(vec, 1.0f / bbd->timestep);
522
523 sub_v3_v3v3(loc, pa->prev_state.co, flbr->oloc);
524
525 mul = dot_v3v3(vec, vec);
526
527 /* leader is not moving */
528 if (mul < 0.01f) {
529 len = len_v3(loc);
530 /* too close to leader */
531 if (len < 2.0f * val->personal_space * pa->size) {
532 copy_v3_v3(bbd->wanted_co, loc);
533 bbd->wanted_speed = val->max_speed;
534 return true;
535 }
536 }
537 else {
538 t = dot_v3v3(loc, vec) / mul;
539
540 /* possible blocking of leader in near future */
541 if (t > 0.0f && t < 3.0f) {
542 copy_v3_v3(vec2, vec);
543 mul_v3_fl(vec2, t);
544
545 sub_v3_v3v3(vec2, loc, vec2);
546
547 len = len_v3(vec2);
548
549 if (len < 2.0f * val->personal_space * pa->size) {
550 copy_v3_v3(bbd->wanted_co, vec2);
551 bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f;
552 return true;
553 }
554 }
555 }
556
557 /* not blocking so try to follow leader */
558 if (p && flbr->options & BRULE_LEADER_IN_LINE) {
559 copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel);
560 copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co);
561 }
562 else {
563 copy_v3_v3(loc, flbr->oloc);
564 sub_v3_v3v3(vec, flbr->loc, flbr->oloc);
565 mul_v3_fl(vec, 1.0f / bbd->timestep);
566 }
567
568 /* fac is seconds behind leader */
569 madd_v3_v3fl(loc, vec, -flbr->distance);
570
571 sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co);
572 bbd->wanted_speed = len_v3(bbd->wanted_co);
573
574 ret = true;
575 }
576 else if (p % n) {
577 float vec2[3], t, t_min = 3.0f;
578
579 /* first check we're not blocking any leaders */
580 for (i = 0; i < bbd->sim->psys->totpart; i += n) {
581 copy_v3_v3(vec, bbd->sim->psys->particles[i].prev_state.vel);
582
583 sub_v3_v3v3(loc, pa->prev_state.co, bbd->sim->psys->particles[i].prev_state.co);
584
585 mul = dot_v3v3(vec, vec);
586
587 /* leader is not moving */
588 if (mul < 0.01f) {
589 len = len_v3(loc);
590 /* too close to leader */
591 if (len < 2.0f * val->personal_space * pa->size) {
592 copy_v3_v3(bbd->wanted_co, loc);
593 bbd->wanted_speed = val->max_speed;
594 return true;
595 }
596 }
597 else {
598 t = dot_v3v3(loc, vec) / mul;
599
600 /* possible blocking of leader in near future */
601 if (t > 0.0f && t < t_min) {
602 copy_v3_v3(vec2, vec);
603 mul_v3_fl(vec2, t);
604
605 sub_v3_v3v3(vec2, loc, vec2);
606
607 len = len_v3(vec2);
608
609 if (len < 2.0f * val->personal_space * pa->size) {
610 t_min = t;
611 copy_v3_v3(bbd->wanted_co, loc);
612 bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f;
613 ret = true;
614 }
615 }
616 }
617 }
618
619 if (ret) {
620 return true;
621 }
622
623 /* not blocking so try to follow leader */
624 if (flbr->options & BRULE_LEADER_IN_LINE) {
625 copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel);
626 copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co);
627 }
628 else {
629 copy_v3_v3(vec, bbd->sim->psys->particles[p - p % n].prev_state.vel);
630 copy_v3_v3(loc, bbd->sim->psys->particles[p - p % n].prev_state.co);
631 }
632
633 /* fac is seconds behind leader */
634 madd_v3_v3fl(loc, vec, -flbr->distance);
635
636 sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co);
637 bbd->wanted_speed = len_v3(bbd->wanted_co);
638
639 ret = true;
640 }
641
642 return ret;
643 }
rule_average_speed(BoidRule * rule,BoidBrainData * bbd,BoidValues * val,ParticleData * pa)644 static bool rule_average_speed(BoidRule *rule,
645 BoidBrainData *bbd,
646 BoidValues *val,
647 ParticleData *pa)
648 {
649 BoidParticle *bpa = pa->boid;
650 BoidRuleAverageSpeed *asbr = (BoidRuleAverageSpeed *)rule;
651 float vec[3] = {0.0f, 0.0f, 0.0f};
652
653 if (asbr->wander > 0.0f) {
654 /* abuse pa->r_ave for wandering */
655 bpa->wander[0] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
656 bpa->wander[1] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
657 bpa->wander[2] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
658
659 normalize_v3(bpa->wander);
660
661 copy_v3_v3(vec, bpa->wander);
662
663 mul_qt_v3(pa->prev_state.rot, vec);
664
665 copy_v3_v3(bbd->wanted_co, pa->prev_state.ave);
666
667 mul_v3_fl(bbd->wanted_co, 1.1f);
668
669 add_v3_v3(bbd->wanted_co, vec);
670
671 /* leveling */
672 if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) {
673 project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity);
674 mul_v3_fl(vec, asbr->level);
675 sub_v3_v3(bbd->wanted_co, vec);
676 }
677 }
678 else {
679 copy_v3_v3(bbd->wanted_co, pa->prev_state.ave);
680
681 /* may happen at birth */
682 if (dot_v2v2(bbd->wanted_co, bbd->wanted_co) == 0.0f) {
683 bbd->wanted_co[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
684 bbd->wanted_co[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
685 bbd->wanted_co[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
686 }
687
688 /* leveling */
689 if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) {
690 project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity);
691 mul_v3_fl(vec, asbr->level);
692 sub_v3_v3(bbd->wanted_co, vec);
693 }
694 }
695 bbd->wanted_speed = asbr->speed * val->max_speed;
696
697 return true;
698 }
rule_fight(BoidRule * rule,BoidBrainData * bbd,BoidValues * val,ParticleData * pa)699 static bool rule_fight(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa)
700 {
701 BoidRuleFight *fbr = (BoidRuleFight *)rule;
702 KDTreeNearest_3d *ptn = NULL;
703 ParticleTarget *pt;
704 ParticleData *epars;
705 ParticleData *enemy_pa = NULL;
706 BoidParticle *bpa;
707 /* friends & enemies */
708 float closest_enemy[3] = {0.0f, 0.0f, 0.0f};
709 float closest_dist = fbr->distance + 1.0f;
710 float f_strength = 0.0f, e_strength = 0.0f;
711 float health = 0.0f;
712 int n;
713 bool ret = false;
714
715 /* calculate own group strength */
716 int neighbors = BLI_kdtree_3d_range_search(
717 bbd->sim->psys->tree, pa->prev_state.co, &ptn, fbr->distance);
718 for (n = 0; n < neighbors; n++) {
719 bpa = bbd->sim->psys->particles[ptn[n].index].boid;
720 health += bpa->data.health;
721 }
722
723 f_strength += bbd->part->boids->strength * health;
724
725 if (ptn) {
726 MEM_freeN(ptn);
727 ptn = NULL;
728 }
729
730 /* add other friendlies and calculate enemy strength and find closest enemy */
731 for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) {
732 ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt);
733 if (epsys) {
734 epars = epsys->particles;
735
736 neighbors = BLI_kdtree_3d_range_search(epsys->tree, pa->prev_state.co, &ptn, fbr->distance);
737
738 health = 0.0f;
739
740 for (n = 0; n < neighbors; n++) {
741 bpa = epars[ptn[n].index].boid;
742 health += bpa->data.health;
743
744 if (n == 0 && pt->mode == PTARGET_MODE_ENEMY && ptn[n].dist < closest_dist) {
745 copy_v3_v3(closest_enemy, ptn[n].co);
746 closest_dist = ptn[n].dist;
747 enemy_pa = epars + ptn[n].index;
748 }
749 }
750 if (pt->mode == PTARGET_MODE_ENEMY) {
751 e_strength += epsys->part->boids->strength * health;
752 }
753 else if (pt->mode == PTARGET_MODE_FRIEND) {
754 f_strength += epsys->part->boids->strength * health;
755 }
756
757 if (ptn) {
758 MEM_freeN(ptn);
759 ptn = NULL;
760 }
761 }
762 }
763 /* decide action if enemy presence found */
764 if (e_strength > 0.0f) {
765 sub_v3_v3v3(bbd->wanted_co, closest_enemy, pa->prev_state.co);
766
767 /* attack if in range */
768 if (closest_dist <= bbd->part->boids->range + pa->size + enemy_pa->size) {
769 float damage = BLI_rng_get_float(bbd->rng);
770 float enemy_dir[3];
771
772 normalize_v3_v3(enemy_dir, bbd->wanted_co);
773
774 /* fight mode */
775 bbd->wanted_speed = 0.0f;
776
777 /* must face enemy to fight */
778 if (dot_v3v3(pa->prev_state.ave, enemy_dir) > 0.5f) {
779 bpa = enemy_pa->boid;
780 bpa->data.health -= bbd->part->boids->strength * bbd->timestep *
781 ((1.0f - bbd->part->boids->accuracy) * damage +
782 bbd->part->boids->accuracy);
783 }
784 }
785 else {
786 /* approach mode */
787 bbd->wanted_speed = val->max_speed;
788 }
789
790 /* check if boid doesn't want to fight */
791 bpa = pa->boid;
792 if (bpa->data.health / bbd->part->boids->health * bbd->part->boids->aggression <
793 e_strength / f_strength) {
794 /* decide to flee */
795 if (closest_dist < fbr->flee_distance * fbr->distance) {
796 negate_v3(bbd->wanted_co);
797 bbd->wanted_speed = val->max_speed;
798 }
799 else { /* wait for better odds */
800 bbd->wanted_speed = 0.0f;
801 }
802 }
803
804 ret = true;
805 }
806
807 return ret;
808 }
809
810 typedef bool (*boid_rule_cb)(BoidRule *rule,
811 BoidBrainData *data,
812 BoidValues *val,
813 ParticleData *pa);
814
815 static boid_rule_cb boid_rules[] = {
816 rule_none,
817 rule_goal_avoid,
818 rule_goal_avoid,
819 rule_avoid_collision,
820 rule_separate,
821 rule_flock,
822 rule_follow_leader,
823 rule_average_speed,
824 rule_fight,
825 // rule_help,
826 // rule_protect,
827 // rule_hide,
828 // rule_follow_path,
829 // rule_follow_wall,
830 };
831
set_boid_values(BoidValues * val,BoidSettings * boids,ParticleData * pa)832 static void set_boid_values(BoidValues *val, BoidSettings *boids, ParticleData *pa)
833 {
834 BoidParticle *bpa = pa->boid;
835
836 if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) {
837 val->max_speed = boids->land_max_speed * bpa->data.health / boids->health;
838 val->max_acc = boids->land_max_acc * val->max_speed;
839 val->max_ave = boids->land_max_ave * (float)M_PI * bpa->data.health / boids->health;
840 val->min_speed = 0.0f; /* no minimum speed on land */
841 val->personal_space = boids->land_personal_space;
842 val->jump_speed = boids->land_jump_speed * bpa->data.health / boids->health;
843 }
844 else {
845 val->max_speed = boids->air_max_speed * bpa->data.health / boids->health;
846 val->max_acc = boids->air_max_acc * val->max_speed;
847 val->max_ave = boids->air_max_ave * (float)M_PI * bpa->data.health / boids->health;
848 val->min_speed = boids->air_min_speed * boids->air_max_speed;
849 val->personal_space = boids->air_personal_space;
850 val->jump_speed = 0.0f; /* no jumping in air */
851 }
852 }
853
boid_find_ground(BoidBrainData * bbd,ParticleData * pa,float ground_co[3],float ground_nor[3])854 static Object *boid_find_ground(BoidBrainData *bbd,
855 ParticleData *pa,
856 float ground_co[3],
857 float ground_nor[3])
858 {
859 const int raycast_flag = BVH_RAYCAST_DEFAULT & ~BVH_RAYCAST_WATERTIGHT;
860 BoidParticle *bpa = pa->boid;
861
862 if (bpa->data.mode == eBoidMode_Climbing) {
863 SurfaceModifierData *surmd = NULL;
864 float x[3], v[3];
865
866 surmd = (SurfaceModifierData *)BKE_modifiers_findby_type(bpa->ground, eModifierType_Surface);
867
868 /* take surface velocity into account */
869 closest_point_on_surface(surmd, pa->state.co, x, NULL, v);
870 add_v3_v3(x, v);
871
872 /* get actual position on surface */
873 closest_point_on_surface(surmd, x, ground_co, ground_nor, NULL);
874
875 return bpa->ground;
876 }
877
878 const float zvec[3] = {0.0f, 0.0f, 2000.0f};
879 ParticleCollision col;
880 ColliderCache *coll;
881 BVHTreeRayHit hit;
882 float radius = 0.0f, t, ray_dir[3];
883
884 if (!bbd->sim->colliders) {
885 return NULL;
886 }
887
888 memset(&col, 0, sizeof(ParticleCollision));
889
890 /* first try to find below boid */
891 copy_v3_v3(col.co1, pa->state.co);
892 sub_v3_v3v3(col.co2, pa->state.co, zvec);
893 sub_v3_v3v3(ray_dir, col.co2, col.co1);
894 col.f = 0.0f;
895 hit.index = -1;
896 hit.dist = col.original_ray_length = normalize_v3(ray_dir);
897 col.pce.inside = 0;
898
899 for (coll = bbd->sim->colliders->first; coll; coll = coll->next) {
900 col.current = coll->ob;
901 col.md = coll->collmd;
902 col.fac1 = col.fac2 = 0.f;
903
904 if (col.md && col.md->bvhtree) {
905 BLI_bvhtree_ray_cast_ex(col.md->bvhtree,
906 col.co1,
907 ray_dir,
908 radius,
909 &hit,
910 BKE_psys_collision_neartest_cb,
911 &col,
912 raycast_flag);
913 }
914 }
915 /* then use that object */
916 if (hit.index >= 0) {
917 t = hit.dist / col.original_ray_length;
918 interp_v3_v3v3(ground_co, col.co1, col.co2, t);
919 normalize_v3_v3(ground_nor, col.pce.nor);
920 return col.hit;
921 }
922
923 /* couldn't find below, so find upmost deflector object */
924 add_v3_v3v3(col.co1, pa->state.co, zvec);
925 sub_v3_v3v3(col.co2, pa->state.co, zvec);
926 sub_v3_v3(col.co2, zvec);
927 sub_v3_v3v3(ray_dir, col.co2, col.co1);
928 col.f = 0.0f;
929 hit.index = -1;
930 hit.dist = col.original_ray_length = normalize_v3(ray_dir);
931
932 for (coll = bbd->sim->colliders->first; coll; coll = coll->next) {
933 col.current = coll->ob;
934 col.md = coll->collmd;
935
936 if (col.md && col.md->bvhtree) {
937 BLI_bvhtree_ray_cast_ex(col.md->bvhtree,
938 col.co1,
939 ray_dir,
940 radius,
941 &hit,
942 BKE_psys_collision_neartest_cb,
943 &col,
944 raycast_flag);
945 }
946 }
947 /* then use that object */
948 if (hit.index >= 0) {
949 t = hit.dist / col.original_ray_length;
950 interp_v3_v3v3(ground_co, col.co1, col.co2, t);
951 normalize_v3_v3(ground_nor, col.pce.nor);
952 return col.hit;
953 }
954
955 /* default to z=0 */
956 copy_v3_v3(ground_co, pa->state.co);
957 ground_co[2] = 0;
958 ground_nor[0] = ground_nor[1] = 0.0f;
959 ground_nor[2] = 1.0f;
960 return NULL;
961 }
boid_rule_applies(ParticleData * pa,BoidSettings * UNUSED (boids),BoidRule * rule)962 static bool boid_rule_applies(ParticleData *pa, BoidSettings *UNUSED(boids), BoidRule *rule)
963 {
964 BoidParticle *bpa = pa->boid;
965
966 if (rule == NULL) {
967 return false;
968 }
969
970 if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) &&
971 rule->flag & BOIDRULE_ON_LAND) {
972 return true;
973 }
974
975 if (bpa->data.mode == eBoidMode_InAir && rule->flag & BOIDRULE_IN_AIR) {
976 return true;
977 }
978
979 return false;
980 }
boids_precalc_rules(ParticleSettings * part,float cfra)981 void boids_precalc_rules(ParticleSettings *part, float cfra)
982 {
983 BoidState *state = part->boids->states.first;
984 BoidRule *rule;
985 for (; state; state = state->next) {
986 for (rule = state->rules.first; rule; rule = rule->next) {
987 if (rule->type == eBoidRuleType_FollowLeader) {
988 BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader *)rule;
989
990 if (flbr->ob && flbr->cfra != cfra) {
991 /* save object locations for velocity calculations */
992 copy_v3_v3(flbr->oloc, flbr->loc);
993 copy_v3_v3(flbr->loc, flbr->ob->obmat[3]);
994 flbr->cfra = cfra;
995 }
996 }
997 }
998 }
999 }
boid_climb(BoidSettings * boids,ParticleData * pa,float * surface_co,float * surface_nor)1000 static void boid_climb(BoidSettings *boids,
1001 ParticleData *pa,
1002 float *surface_co,
1003 float *surface_nor)
1004 {
1005 BoidParticle *bpa = pa->boid;
1006 float nor[3], vel[3];
1007 copy_v3_v3(nor, surface_nor);
1008
1009 /* gather apparent gravity */
1010 madd_v3_v3fl(bpa->gravity, surface_nor, -1.0f);
1011 normalize_v3(bpa->gravity);
1012
1013 /* raise boid it's size from surface */
1014 mul_v3_fl(nor, pa->size * boids->height);
1015 add_v3_v3v3(pa->state.co, surface_co, nor);
1016
1017 /* remove normal component from velocity */
1018 project_v3_v3v3(vel, pa->state.vel, surface_nor);
1019 sub_v3_v3v3(pa->state.vel, pa->state.vel, vel);
1020 }
boid_goal_signed_dist(float * boid_co,float * goal_co,float * goal_nor)1021 static float boid_goal_signed_dist(float *boid_co, float *goal_co, float *goal_nor)
1022 {
1023 float vec[3];
1024
1025 sub_v3_v3v3(vec, boid_co, goal_co);
1026
1027 return dot_v3v3(vec, goal_nor);
1028 }
1029 /* wanted_co is relative to boid location */
apply_boid_rule(BoidBrainData * bbd,BoidRule * rule,BoidValues * val,ParticleData * pa,float fuzziness)1030 static bool apply_boid_rule(
1031 BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness)
1032 {
1033 if (rule == NULL) {
1034 return false;
1035 }
1036
1037 if (!boid_rule_applies(pa, bbd->part->boids, rule)) {
1038 return false;
1039 }
1040
1041 if (!boid_rules[rule->type](rule, bbd, val, pa)) {
1042 return false;
1043 }
1044
1045 if (fuzziness < 0.0f || compare_len_v3v3(bbd->wanted_co,
1046 pa->prev_state.vel,
1047 fuzziness * len_v3(pa->prev_state.vel)) == 0) {
1048 return true;
1049 }
1050 return false;
1051 }
get_boid_state(BoidSettings * boids,ParticleData * pa)1052 static BoidState *get_boid_state(BoidSettings *boids, ParticleData *pa)
1053 {
1054 BoidState *state = boids->states.first;
1055 BoidParticle *bpa = pa->boid;
1056
1057 for (; state; state = state->next) {
1058 if (state->id == bpa->data.state_id) {
1059 return state;
1060 }
1061 }
1062
1063 /* for some reason particle isn't at a valid state */
1064 state = boids->states.first;
1065 if (state) {
1066 bpa->data.state_id = state->id;
1067 }
1068
1069 return state;
1070 }
1071 // static int boid_condition_is_true(BoidCondition *cond)
1072 //{
1073 // /* TODO */
1074 // return 0;
1075 //}
1076
1077 /* determines the velocity the boid wants to have */
boid_brain(BoidBrainData * bbd,int p,ParticleData * pa)1078 void boid_brain(BoidBrainData *bbd, int p, ParticleData *pa)
1079 {
1080 BoidRule *rule;
1081 BoidSettings *boids = bbd->part->boids;
1082 BoidValues val;
1083 BoidState *state = get_boid_state(boids, pa);
1084 BoidParticle *bpa = pa->boid;
1085 ParticleSystem *psys = bbd->sim->psys;
1086 int rand;
1087 // BoidCondition *cond;
1088
1089 if (bpa->data.health <= 0.0f) {
1090 pa->alive = PARS_DYING;
1091 pa->dietime = bbd->cfra;
1092 return;
1093 }
1094
1095 // planned for near future
1096 // cond = state->conditions.first;
1097 // for (; cond; cond=cond->next) {
1098 // if (boid_condition_is_true(cond)) {
1099 // pa->boid->state_id = cond->state_id;
1100 // state = get_boid_state(boids, pa);
1101 // break; /* only first true condition is used */
1102 // }
1103 //}
1104
1105 zero_v3(bbd->wanted_co);
1106 bbd->wanted_speed = 0.0f;
1107
1108 /* create random seed for every particle & frame */
1109 rand = (int)(psys_frand(psys, psys->seed + p) * 1000);
1110 rand = (int)(psys_frand(psys, (int)bbd->cfra + rand) * 1000);
1111
1112 set_boid_values(&val, bbd->part->boids, pa);
1113
1114 /* go through rules */
1115 switch (state->ruleset_type) {
1116 case eBoidRulesetType_Fuzzy: {
1117 for (rule = state->rules.first; rule; rule = rule->next) {
1118 if (apply_boid_rule(bbd, rule, &val, pa, state->rule_fuzziness)) {
1119 break; /* only first nonzero rule that comes through fuzzy rule is applied */
1120 }
1121 }
1122 break;
1123 }
1124 case eBoidRulesetType_Random: {
1125 /* use random rule for each particle (always same for same particle though) */
1126 const int n = BLI_listbase_count(&state->rules);
1127 if (n) {
1128 rule = BLI_findlink(&state->rules, rand % n);
1129 apply_boid_rule(bbd, rule, &val, pa, -1.0);
1130 }
1131 break;
1132 }
1133 case eBoidRulesetType_Average: {
1134 float wanted_co[3] = {0.0f, 0.0f, 0.0f}, wanted_speed = 0.0f;
1135 int n = 0;
1136 for (rule = state->rules.first; rule; rule = rule->next) {
1137 if (apply_boid_rule(bbd, rule, &val, pa, -1.0f)) {
1138 add_v3_v3(wanted_co, bbd->wanted_co);
1139 wanted_speed += bbd->wanted_speed;
1140 n++;
1141 zero_v3(bbd->wanted_co);
1142 bbd->wanted_speed = 0.0f;
1143 }
1144 }
1145
1146 if (n > 1) {
1147 mul_v3_fl(wanted_co, 1.0f / (float)n);
1148 wanted_speed /= (float)n;
1149 }
1150
1151 copy_v3_v3(bbd->wanted_co, wanted_co);
1152 bbd->wanted_speed = wanted_speed;
1153 break;
1154 }
1155 }
1156
1157 /* decide on jumping & liftoff */
1158 if (bpa->data.mode == eBoidMode_OnLand) {
1159 /* fuzziness makes boids capable of misjudgement */
1160 float mul = 1.0f + state->rule_fuzziness;
1161
1162 if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) {
1163 float cvel[3], dir[3];
1164
1165 copy_v3_v3(dir, pa->prev_state.ave);
1166 normalize_v2(dir);
1167
1168 copy_v3_v3(cvel, bbd->wanted_co);
1169 normalize_v2(cvel);
1170
1171 if (dot_v2v2(cvel, dir) > 0.95f / mul) {
1172 bpa->data.mode = eBoidMode_Liftoff;
1173 }
1174 }
1175 else if (val.jump_speed > 0.0f) {
1176 float jump_v[3];
1177 int jump = 0;
1178
1179 /* jump to get to a location */
1180 if (bbd->wanted_co[2] > 0.0f) {
1181 float cvel[3], dir[3];
1182 float z_v, ground_v, cur_v;
1183 float len;
1184
1185 copy_v3_v3(dir, pa->prev_state.ave);
1186 normalize_v2(dir);
1187
1188 copy_v3_v3(cvel, bbd->wanted_co);
1189 normalize_v2(cvel);
1190
1191 len = len_v2(pa->prev_state.vel);
1192
1193 /* first of all, are we going in a suitable direction? */
1194 /* or at a suitably slow speed */
1195 if (dot_v2v2(cvel, dir) > 0.95f / mul || len <= state->rule_fuzziness) {
1196 /* try to reach goal at highest point of the parabolic path */
1197 cur_v = len_v2(pa->prev_state.vel);
1198 z_v = sasqrt(-2.0f * bbd->sim->scene->physics_settings.gravity[2] * bbd->wanted_co[2]);
1199 ground_v = len_v2(bbd->wanted_co) *
1200 sasqrt(-0.5f * bbd->sim->scene->physics_settings.gravity[2] /
1201 bbd->wanted_co[2]);
1202
1203 len = sasqrt((ground_v - cur_v) * (ground_v - cur_v) + z_v * z_v);
1204
1205 if (len < val.jump_speed * mul || bbd->part->boids->options & BOID_ALLOW_FLIGHT) {
1206 jump = 1;
1207
1208 len = MIN2(len, val.jump_speed);
1209
1210 copy_v3_v3(jump_v, dir);
1211 jump_v[2] = z_v;
1212 mul_v3_fl(jump_v, ground_v);
1213
1214 normalize_v3(jump_v);
1215 mul_v3_fl(jump_v, len);
1216 add_v2_v2v2(jump_v, jump_v, pa->prev_state.vel);
1217 }
1218 }
1219 }
1220
1221 /* jump to go faster */
1222 if (jump == 0 && val.jump_speed > val.max_speed && bbd->wanted_speed > val.max_speed) {
1223 /* pass */
1224 }
1225
1226 if (jump) {
1227 copy_v3_v3(pa->prev_state.vel, jump_v);
1228 bpa->data.mode = eBoidMode_Falling;
1229 }
1230 }
1231 }
1232 }
1233 /* tries to realize the wanted velocity taking all constraints into account */
boid_body(BoidBrainData * bbd,ParticleData * pa)1234 void boid_body(BoidBrainData *bbd, ParticleData *pa)
1235 {
1236 BoidSettings *boids = bbd->part->boids;
1237 BoidParticle *bpa = pa->boid;
1238 BoidValues val;
1239 EffectedPoint epoint;
1240 float acc[3] = {0.0f, 0.0f, 0.0f}, tan_acc[3], nor_acc[3];
1241 float dvec[3], bvec[3];
1242 float new_dir[3], new_speed;
1243 float old_dir[3], old_speed;
1244 float wanted_dir[3];
1245 float q[4], mat[3][3]; /* rotation */
1246 float ground_co[3] = {0.0f, 0.0f, 0.0f}, ground_nor[3] = {0.0f, 0.0f, 1.0f};
1247 float force[3] = {0.0f, 0.0f, 0.0f};
1248 float pa_mass = bbd->part->mass, dtime = bbd->dfra * bbd->timestep;
1249
1250 set_boid_values(&val, boids, pa);
1251
1252 /* make sure there's something in new velocity, location & rotation */
1253 copy_particle_key(&pa->state, &pa->prev_state, 0);
1254
1255 if (bbd->part->flag & PART_SIZEMASS) {
1256 pa_mass *= pa->size;
1257 }
1258
1259 /* if boids can't fly they fall to the ground */
1260 if ((boids->options & BOID_ALLOW_FLIGHT) == 0 &&
1261 ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) == 0 &&
1262 psys_uses_gravity(bbd->sim)) {
1263 bpa->data.mode = eBoidMode_Falling;
1264 }
1265
1266 if (bpa->data.mode == eBoidMode_Falling) {
1267 /* Falling boids are only effected by gravity. */
1268 acc[2] = bbd->sim->scene->physics_settings.gravity[2];
1269 }
1270 else {
1271 /* figure out acceleration */
1272 float landing_level = 2.0f;
1273 float level = landing_level + 1.0f;
1274 float new_vel[3];
1275
1276 if (bpa->data.mode == eBoidMode_Liftoff) {
1277 bpa->data.mode = eBoidMode_InAir;
1278 bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor);
1279 }
1280 else if (bpa->data.mode == eBoidMode_InAir && boids->options & BOID_ALLOW_LAND) {
1281 /* auto-leveling & landing if close to ground */
1282
1283 bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor);
1284
1285 /* level = how many particle sizes above ground */
1286 level = (pa->prev_state.co[2] - ground_co[2]) / (2.0f * pa->size) - 0.5f;
1287
1288 landing_level = -boids->landing_smoothness * pa->prev_state.vel[2] * pa_mass;
1289
1290 if (pa->prev_state.vel[2] < 0.0f) {
1291 if (level < 1.0f) {
1292 bbd->wanted_co[0] = bbd->wanted_co[1] = bbd->wanted_co[2] = 0.0f;
1293 bbd->wanted_speed = 0.0f;
1294 bpa->data.mode = eBoidMode_Falling;
1295 }
1296 else if (level < landing_level) {
1297 bbd->wanted_speed *= (level - 1.0f) / landing_level;
1298 bbd->wanted_co[2] *= (level - 1.0f) / landing_level;
1299 }
1300 }
1301 }
1302
1303 copy_v3_v3(old_dir, pa->prev_state.ave);
1304 new_speed = normalize_v3_v3(wanted_dir, bbd->wanted_co);
1305
1306 /* first check if we have valid direction we want to go towards */
1307 if (new_speed == 0.0f) {
1308 copy_v3_v3(new_dir, old_dir);
1309 }
1310 else {
1311 float old_dir2[2], wanted_dir2[2], nor[3], angle;
1312 copy_v2_v2(old_dir2, old_dir);
1313 normalize_v2(old_dir2);
1314 copy_v2_v2(wanted_dir2, wanted_dir);
1315 normalize_v2(wanted_dir2);
1316
1317 /* choose random direction to turn if wanted velocity */
1318 /* is directly behind regardless of z-coordinate */
1319 if (dot_v2v2(old_dir2, wanted_dir2) < -0.99f) {
1320 wanted_dir[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1321 wanted_dir[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1322 wanted_dir[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1323 normalize_v3(wanted_dir);
1324 }
1325
1326 /* constrain direction with maximum angular velocity */
1327 angle = saacos(dot_v3v3(old_dir, wanted_dir));
1328 angle = min_ff(angle, val.max_ave);
1329
1330 cross_v3_v3v3(nor, old_dir, wanted_dir);
1331 axis_angle_to_quat(q, nor, angle);
1332 copy_v3_v3(new_dir, old_dir);
1333 mul_qt_v3(q, new_dir);
1334 normalize_v3(new_dir);
1335
1336 /* save direction in case resulting velocity too small */
1337 axis_angle_to_quat(q, nor, angle * dtime);
1338 copy_v3_v3(pa->state.ave, old_dir);
1339 mul_qt_v3(q, pa->state.ave);
1340 normalize_v3(pa->state.ave);
1341 }
1342
1343 /* constrain speed with maximum acceleration */
1344 old_speed = len_v3(pa->prev_state.vel);
1345
1346 if (bbd->wanted_speed < old_speed) {
1347 new_speed = MAX2(bbd->wanted_speed, old_speed - val.max_acc);
1348 }
1349 else {
1350 new_speed = MIN2(bbd->wanted_speed, old_speed + val.max_acc);
1351 }
1352
1353 /* combine direction and speed */
1354 copy_v3_v3(new_vel, new_dir);
1355 mul_v3_fl(new_vel, new_speed);
1356
1357 /* maintain minimum flying velocity if not landing */
1358 if (level >= landing_level) {
1359 float len2 = dot_v2v2(new_vel, new_vel);
1360 float root;
1361
1362 len2 = MAX2(len2, val.min_speed * val.min_speed);
1363 root = sasqrt(new_speed * new_speed - len2);
1364
1365 new_vel[2] = new_vel[2] < 0.0f ? -root : root;
1366
1367 normalize_v2(new_vel);
1368 mul_v2_fl(new_vel, sasqrt(len2));
1369 }
1370
1371 /* finally constrain speed to max speed */
1372 new_speed = normalize_v3(new_vel);
1373 mul_v3_fl(new_vel, MIN2(new_speed, val.max_speed));
1374
1375 /* get acceleration from difference of velocities */
1376 sub_v3_v3v3(acc, new_vel, pa->prev_state.vel);
1377
1378 /* break acceleration to components */
1379 project_v3_v3v3(tan_acc, acc, pa->prev_state.ave);
1380 sub_v3_v3v3(nor_acc, acc, tan_acc);
1381 }
1382
1383 /* account for effectors */
1384 pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint);
1385 BKE_effectors_apply(bbd->sim->psys->effectors,
1386 bbd->sim->colliders,
1387 bbd->part->effector_weights,
1388 &epoint,
1389 force,
1390 NULL,
1391 NULL);
1392
1393 if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) {
1394 float length = normalize_v3(force);
1395
1396 length = MAX2(0.0f, length - boids->land_stick_force);
1397
1398 mul_v3_fl(force, length);
1399 }
1400
1401 add_v3_v3(acc, force);
1402
1403 /* store smoothed acceleration for nice banking etc. */
1404 madd_v3_v3fl(bpa->data.acc, acc, dtime);
1405 mul_v3_fl(bpa->data.acc, 1.0f / (1.0f + dtime));
1406
1407 /* integrate new location & velocity */
1408
1409 /* by regarding the acceleration as a force at this stage we
1410 * can get better control although it's a bit unphysical */
1411 mul_v3_fl(acc, 1.0f / pa_mass);
1412
1413 copy_v3_v3(dvec, acc);
1414 mul_v3_fl(dvec, dtime * dtime * 0.5f);
1415
1416 copy_v3_v3(bvec, pa->prev_state.vel);
1417 mul_v3_fl(bvec, dtime);
1418 add_v3_v3(dvec, bvec);
1419 add_v3_v3(pa->state.co, dvec);
1420
1421 madd_v3_v3fl(pa->state.vel, acc, dtime);
1422
1423 // if (bpa->data.mode != eBoidMode_InAir)
1424 bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor);
1425
1426 /* change modes, constrain movement & keep track of down vector */
1427 switch (bpa->data.mode) {
1428 case eBoidMode_InAir: {
1429 float grav[3];
1430
1431 grav[0] = 0.0f;
1432 grav[1] = 0.0f;
1433 grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f;
1434
1435 /* don't take forward acceleration into account (better banking) */
1436 if (dot_v3v3(bpa->data.acc, pa->state.vel) > 0.0f) {
1437 project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel);
1438 sub_v3_v3v3(dvec, bpa->data.acc, dvec);
1439 }
1440 else {
1441 copy_v3_v3(dvec, bpa->data.acc);
1442 }
1443
1444 /* gather apparent gravity */
1445 madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking);
1446 normalize_v3(bpa->gravity);
1447
1448 /* stick boid on goal when close enough */
1449 if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <=
1450 pa->size * boids->height) {
1451 bpa->data.mode = eBoidMode_Climbing;
1452 bpa->ground = bbd->goal_ob;
1453 boid_find_ground(bbd, pa, ground_co, ground_nor);
1454 boid_climb(boids, pa, ground_co, ground_nor);
1455 }
1456 else if (pa->state.co[2] <= ground_co[2] + pa->size * boids->height) {
1457 /* land boid when below ground */
1458 if (boids->options & BOID_ALLOW_LAND) {
1459 pa->state.co[2] = ground_co[2] + pa->size * boids->height;
1460 pa->state.vel[2] = 0.0f;
1461 bpa->data.mode = eBoidMode_OnLand;
1462 }
1463 /* fly above ground */
1464 else if (bpa->ground) {
1465 pa->state.co[2] = ground_co[2] + pa->size * boids->height;
1466 pa->state.vel[2] = 0.0f;
1467 }
1468 }
1469 break;
1470 }
1471 case eBoidMode_Falling: {
1472 float grav[3];
1473
1474 grav[0] = 0.0f;
1475 grav[1] = 0.0f;
1476 grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f;
1477
1478 /* gather apparent gravity */
1479 madd_v3_v3fl(bpa->gravity, grav, dtime);
1480 normalize_v3(bpa->gravity);
1481
1482 if (boids->options & BOID_ALLOW_LAND) {
1483 /* stick boid on goal when close enough */
1484 if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <=
1485 pa->size * boids->height) {
1486 bpa->data.mode = eBoidMode_Climbing;
1487 bpa->ground = bbd->goal_ob;
1488 boid_find_ground(bbd, pa, ground_co, ground_nor);
1489 boid_climb(boids, pa, ground_co, ground_nor);
1490 }
1491 /* land boid when really near ground */
1492 else if (pa->state.co[2] <= ground_co[2] + 1.01f * pa->size * boids->height) {
1493 pa->state.co[2] = ground_co[2] + pa->size * boids->height;
1494 pa->state.vel[2] = 0.0f;
1495 bpa->data.mode = eBoidMode_OnLand;
1496 }
1497 /* if we're falling, can fly and want to go upwards lets fly */
1498 else if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) {
1499 bpa->data.mode = eBoidMode_InAir;
1500 }
1501 }
1502 else {
1503 bpa->data.mode = eBoidMode_InAir;
1504 }
1505 break;
1506 }
1507 case eBoidMode_Climbing: {
1508 boid_climb(boids, pa, ground_co, ground_nor);
1509 // float nor[3];
1510 // copy_v3_v3(nor, ground_nor);
1511
1512 ///* gather apparent gravity to r_ve */
1513 // madd_v3_v3fl(pa->r_ve, ground_nor, -1.0);
1514 // normalize_v3(pa->r_ve);
1515
1516 ///* raise boid it's size from surface */
1517 // mul_v3_fl(nor, pa->size * boids->height);
1518 // add_v3_v3v3(pa->state.co, ground_co, nor);
1519
1520 ///* remove normal component from velocity */
1521 // project_v3_v3v3(v, pa->state.vel, ground_nor);
1522 // sub_v3_v3v3(pa->state.vel, pa->state.vel, v);
1523 break;
1524 }
1525 case eBoidMode_OnLand: {
1526 /* stick boid on goal when close enough */
1527 if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <=
1528 pa->size * boids->height) {
1529 bpa->data.mode = eBoidMode_Climbing;
1530 bpa->ground = bbd->goal_ob;
1531 boid_find_ground(bbd, pa, ground_co, ground_nor);
1532 boid_climb(boids, pa, ground_co, ground_nor);
1533 }
1534 /* ground is too far away so boid falls */
1535 else if (pa->state.co[2] - ground_co[2] > 1.1f * pa->size * boids->height) {
1536 bpa->data.mode = eBoidMode_Falling;
1537 }
1538 else {
1539 /* constrain to surface */
1540 pa->state.co[2] = ground_co[2] + pa->size * boids->height;
1541 pa->state.vel[2] = 0.0f;
1542 }
1543
1544 if (boids->banking > 0.0f) {
1545 float grav[3];
1546 /* Don't take gravity's strength in to account, */
1547 /* otherwise amount of banking is hard to control. */
1548 negate_v3_v3(grav, ground_nor);
1549
1550 project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel);
1551 sub_v3_v3v3(dvec, bpa->data.acc, dvec);
1552
1553 /* gather apparent gravity */
1554 madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking);
1555 normalize_v3(bpa->gravity);
1556 }
1557 else {
1558 /* gather negative surface normal */
1559 madd_v3_v3fl(bpa->gravity, ground_nor, -1.0f);
1560 normalize_v3(bpa->gravity);
1561 }
1562 break;
1563 }
1564 }
1565
1566 /* save direction to state.ave unless the boid is falling */
1567 /* (boids can't effect their direction when falling) */
1568 if (bpa->data.mode != eBoidMode_Falling && len_v3(pa->state.vel) > 0.1f * pa->size) {
1569 copy_v3_v3(pa->state.ave, pa->state.vel);
1570 pa->state.ave[2] *= bbd->part->boids->pitch;
1571 normalize_v3(pa->state.ave);
1572 }
1573
1574 /* apply damping */
1575 if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) {
1576 mul_v3_fl(pa->state.vel, 1.0f - 0.2f * bbd->part->dampfac);
1577 }
1578
1579 /* calculate rotation matrix based on forward & down vectors */
1580 if (bpa->data.mode == eBoidMode_InAir) {
1581 copy_v3_v3(mat[0], pa->state.ave);
1582
1583 project_v3_v3v3(dvec, bpa->gravity, pa->state.ave);
1584 sub_v3_v3v3(mat[2], bpa->gravity, dvec);
1585 normalize_v3(mat[2]);
1586 }
1587 else {
1588 project_v3_v3v3(dvec, pa->state.ave, bpa->gravity);
1589 sub_v3_v3v3(mat[0], pa->state.ave, dvec);
1590 normalize_v3(mat[0]);
1591
1592 copy_v3_v3(mat[2], bpa->gravity);
1593 }
1594 negate_v3(mat[2]);
1595 cross_v3_v3v3(mat[1], mat[2], mat[0]);
1596
1597 /* apply rotation */
1598 mat3_to_quat_is_ok(q, mat);
1599 copy_qt_qt(pa->state.rot, q);
1600 }
1601
boid_new_rule(int type)1602 BoidRule *boid_new_rule(int type)
1603 {
1604 BoidRule *rule = NULL;
1605 if (type <= 0) {
1606 return NULL;
1607 }
1608
1609 switch (type) {
1610 case eBoidRuleType_Goal:
1611 case eBoidRuleType_Avoid:
1612 rule = MEM_callocN(sizeof(BoidRuleGoalAvoid), "BoidRuleGoalAvoid");
1613 break;
1614 case eBoidRuleType_AvoidCollision:
1615 rule = MEM_callocN(sizeof(BoidRuleAvoidCollision), "BoidRuleAvoidCollision");
1616 ((BoidRuleAvoidCollision *)rule)->look_ahead = 2.0f;
1617 break;
1618 case eBoidRuleType_FollowLeader:
1619 rule = MEM_callocN(sizeof(BoidRuleFollowLeader), "BoidRuleFollowLeader");
1620 ((BoidRuleFollowLeader *)rule)->distance = 1.0f;
1621 break;
1622 case eBoidRuleType_AverageSpeed:
1623 rule = MEM_callocN(sizeof(BoidRuleAverageSpeed), "BoidRuleAverageSpeed");
1624 ((BoidRuleAverageSpeed *)rule)->speed = 0.5f;
1625 break;
1626 case eBoidRuleType_Fight:
1627 rule = MEM_callocN(sizeof(BoidRuleFight), "BoidRuleFight");
1628 ((BoidRuleFight *)rule)->distance = 100.0f;
1629 ((BoidRuleFight *)rule)->flee_distance = 100.0f;
1630 break;
1631 default:
1632 rule = MEM_callocN(sizeof(BoidRule), "BoidRule");
1633 break;
1634 }
1635
1636 rule->type = type;
1637 rule->flag |= BOIDRULE_IN_AIR | BOIDRULE_ON_LAND;
1638 BLI_strncpy(rule->name, rna_enum_boidrule_type_items[type - 1].name, sizeof(rule->name));
1639
1640 return rule;
1641 }
boid_default_settings(BoidSettings * boids)1642 void boid_default_settings(BoidSettings *boids)
1643 {
1644 boids->air_max_speed = 10.0f;
1645 boids->air_max_acc = 0.5f;
1646 boids->air_max_ave = 0.5f;
1647 boids->air_personal_space = 1.0f;
1648
1649 boids->land_max_speed = 5.0f;
1650 boids->land_max_acc = 0.5f;
1651 boids->land_max_ave = 0.5f;
1652 boids->land_personal_space = 1.0f;
1653
1654 boids->options = BOID_ALLOW_FLIGHT;
1655
1656 boids->landing_smoothness = 3.0f;
1657 boids->banking = 1.0f;
1658 boids->pitch = 1.0f;
1659 boids->height = 1.0f;
1660
1661 boids->health = 1.0f;
1662 boids->accuracy = 1.0f;
1663 boids->aggression = 2.0f;
1664 boids->range = 1.0f;
1665 boids->strength = 0.1f;
1666 }
1667
boid_new_state(BoidSettings * boids)1668 BoidState *boid_new_state(BoidSettings *boids)
1669 {
1670 BoidState *state = MEM_callocN(sizeof(BoidState), "BoidState");
1671
1672 state->id = boids->last_state_id++;
1673 if (state->id) {
1674 BLI_snprintf(state->name, sizeof(state->name), "State %i", state->id);
1675 }
1676 else {
1677 strcpy(state->name, "State");
1678 }
1679
1680 state->rule_fuzziness = 0.5;
1681 state->volume = 1.0f;
1682 state->channels |= ~0;
1683
1684 return state;
1685 }
1686
boid_duplicate_state(BoidSettings * boids,BoidState * state)1687 BoidState *boid_duplicate_state(BoidSettings *boids, BoidState *state)
1688 {
1689 BoidState *staten = MEM_dupallocN(state);
1690
1691 BLI_duplicatelist(&staten->rules, &state->rules);
1692 BLI_duplicatelist(&staten->conditions, &state->conditions);
1693 BLI_duplicatelist(&staten->actions, &state->actions);
1694
1695 staten->id = boids->last_state_id++;
1696
1697 return staten;
1698 }
boid_free_settings(BoidSettings * boids)1699 void boid_free_settings(BoidSettings *boids)
1700 {
1701 if (boids) {
1702 BoidState *state = boids->states.first;
1703
1704 for (; state; state = state->next) {
1705 BLI_freelistN(&state->rules);
1706 BLI_freelistN(&state->conditions);
1707 BLI_freelistN(&state->actions);
1708 }
1709
1710 BLI_freelistN(&boids->states);
1711
1712 MEM_freeN(boids);
1713 }
1714 }
boid_copy_settings(const BoidSettings * boids)1715 BoidSettings *boid_copy_settings(const BoidSettings *boids)
1716 {
1717 BoidSettings *nboids = NULL;
1718
1719 if (boids) {
1720 BoidState *state;
1721 BoidState *nstate;
1722
1723 nboids = MEM_dupallocN(boids);
1724
1725 BLI_duplicatelist(&nboids->states, &boids->states);
1726
1727 state = boids->states.first;
1728 nstate = nboids->states.first;
1729 for (; state; state = state->next, nstate = nstate->next) {
1730 BLI_duplicatelist(&nstate->rules, &state->rules);
1731 BLI_duplicatelist(&nstate->conditions, &state->conditions);
1732 BLI_duplicatelist(&nstate->actions, &state->actions);
1733 }
1734 }
1735
1736 return nboids;
1737 }
boid_get_current_state(BoidSettings * boids)1738 BoidState *boid_get_current_state(BoidSettings *boids)
1739 {
1740 BoidState *state = boids->states.first;
1741
1742 for (; state; state = state->next) {
1743 if (state->flag & BOIDSTATE_CURRENT) {
1744 break;
1745 }
1746 }
1747
1748 return state;
1749 }
1750