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