Cleanup: style, use braces for blenkernel
[blender.git] / source / blender / blenkernel / intern / boids.c
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 <string.h>
25 #include <math.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_rand.h"
33 #include "BLI_math.h"
34 #include "BLI_blenlib.h"
35 #include "BLI_kdtree.h"
36 #include "BLI_utildefines.h"
37
38 #include "BKE_collision.h"
39 #include "BKE_effect.h"
40 #include "BKE_boids.h"
41 #include "BKE_particle.h"
42
43 #include "BKE_modifier.h"
44
45 #include "RNA_enum_types.h"
46
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 int apply_boid_rule(
72     BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness);
73
74 static int rule_none(BoidRule *UNUSED(rule),
75                      BoidBrainData *UNUSED(data),
76                      BoidValues *UNUSED(val),
77                      ParticleData *UNUSED(pa))
78 {
79   return 0;
80 }
81
82 static int 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   int ret = 0;
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 = 1;
211   }
212
213   return ret;
214 }
215
216 static int 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   int 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 1;
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 }
416 static int 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   int ret = 0;
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 = 1;
457       }
458
459       if (ptn) {
460         MEM_freeN(ptn);
461         ptn = NULL;
462       }
463     }
464   }
465   return ret;
466 }
467 static int 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   int ret = 0;
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 = 1;
501   }
502   return ret;
503 }
504 static int 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, ret = 0, p = pa - bbd->sim->psys->particles;
514
515   if (flbr->ob) {
516     float vec2[3], t;
517
518     /* first check we're not blocking the leader */
519     sub_v3_v3v3(vec, flbr->loc, flbr->oloc);
520     mul_v3_fl(vec, 1.0f / bbd->timestep);
521
522     sub_v3_v3v3(loc, pa->prev_state.co, flbr->oloc);
523
524     mul = dot_v3v3(vec, vec);
525
526     /* leader is not moving */
527     if (mul < 0.01f) {
528       len = len_v3(loc);
529       /* too close to leader */
530       if (len < 2.0f * val->personal_space * pa->size) {
531         copy_v3_v3(bbd->wanted_co, loc);
532         bbd->wanted_speed = val->max_speed;
533         return 1;
534       }
535     }
536     else {
537       t = dot_v3v3(loc, vec) / mul;
538
539       /* possible blocking of leader in near future */
540       if (t > 0.0f && t < 3.0f) {
541         copy_v3_v3(vec2, vec);
542         mul_v3_fl(vec2, t);
543
544         sub_v3_v3v3(vec2, loc, vec2);
545
546         len = len_v3(vec2);
547
548         if (len < 2.0f * val->personal_space * pa->size) {
549           copy_v3_v3(bbd->wanted_co, vec2);
550           bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f;
551           return 1;
552         }
553       }
554     }
555
556     /* not blocking so try to follow leader */
557     if (p && flbr->options & BRULE_LEADER_IN_LINE) {
558       copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel);
559       copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co);
560     }
561     else {
562       copy_v3_v3(loc, flbr->oloc);
563       sub_v3_v3v3(vec, flbr->loc, flbr->oloc);
564       mul_v3_fl(vec, 1.0f / bbd->timestep);
565     }
566
567     /* fac is seconds behind leader */
568     madd_v3_v3fl(loc, vec, -flbr->distance);
569
570     sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co);
571     bbd->wanted_speed = len_v3(bbd->wanted_co);
572
573     ret = 1;
574   }
575   else if (p % n) {
576     float vec2[3], t, t_min = 3.0f;
577
578     /* first check we're not blocking any leaders */
579     for (i = 0; i < bbd->sim->psys->totpart; i += n) {
580       copy_v3_v3(vec, bbd->sim->psys->particles[i].prev_state.vel);
581
582       sub_v3_v3v3(loc, pa->prev_state.co, bbd->sim->psys->particles[i].prev_state.co);
583
584       mul = dot_v3v3(vec, vec);
585
586       /* leader is not moving */
587       if (mul < 0.01f) {
588         len = len_v3(loc);
589         /* too close to leader */
590         if (len < 2.0f * val->personal_space * pa->size) {
591           copy_v3_v3(bbd->wanted_co, loc);
592           bbd->wanted_speed = val->max_speed;
593           return 1;
594         }
595       }
596       else {
597         t = dot_v3v3(loc, vec) / mul;
598
599         /* possible blocking of leader in near future */
600         if (t > 0.0f && t < t_min) {
601           copy_v3_v3(vec2, vec);
602           mul_v3_fl(vec2, t);
603
604           sub_v3_v3v3(vec2, loc, vec2);
605
606           len = len_v3(vec2);
607
608           if (len < 2.0f * val->personal_space * pa->size) {
609             t_min = t;
610             copy_v3_v3(bbd->wanted_co, loc);
611             bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f;
612             ret = 1;
613           }
614         }
615       }
616     }
617
618     if (ret) {
619       return 1;
620     }
621
622     /* not blocking so try to follow leader */
623     if (flbr->options & BRULE_LEADER_IN_LINE) {
624       copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel);
625       copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co);
626     }
627     else {
628       copy_v3_v3(vec, bbd->sim->psys->particles[p - p % n].prev_state.vel);
629       copy_v3_v3(loc, bbd->sim->psys->particles[p - p % n].prev_state.co);
630     }
631
632     /* fac is seconds behind leader */
633     madd_v3_v3fl(loc, vec, -flbr->distance);
634
635     sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co);
636     bbd->wanted_speed = len_v3(bbd->wanted_co);
637
638     ret = 1;
639   }
640
641   return ret;
642 }
643 static int rule_average_speed(BoidRule *rule,
644                               BoidBrainData *bbd,
645                               BoidValues *val,
646                               ParticleData *pa)
647 {
648   BoidParticle *bpa = pa->boid;
649   BoidRuleAverageSpeed *asbr = (BoidRuleAverageSpeed *)rule;
650   float vec[3] = {0.0f, 0.0f, 0.0f};
651
652   if (asbr->wander > 0.0f) {
653     /* abuse pa->r_ave for wandering */
654     bpa->wander[0] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
655     bpa->wander[1] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
656     bpa->wander[2] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng));
657
658     normalize_v3(bpa->wander);
659
660     copy_v3_v3(vec, bpa->wander);
661
662     mul_qt_v3(pa->prev_state.rot, vec);
663
664     copy_v3_v3(bbd->wanted_co, pa->prev_state.ave);
665
666     mul_v3_fl(bbd->wanted_co, 1.1f);
667
668     add_v3_v3(bbd->wanted_co, vec);
669
670     /* leveling */
671     if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) {
672       project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity);
673       mul_v3_fl(vec, asbr->level);
674       sub_v3_v3(bbd->wanted_co, vec);
675     }
676   }
677   else {
678     copy_v3_v3(bbd->wanted_co, pa->prev_state.ave);
679
680     /* may happen at birth */
681     if (dot_v2v2(bbd->wanted_co, bbd->wanted_co) == 0.0f) {
682       bbd->wanted_co[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
683       bbd->wanted_co[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
684       bbd->wanted_co[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
685     }
686
687     /* leveling */
688     if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) {
689       project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity);
690       mul_v3_fl(vec, asbr->level);
691       sub_v3_v3(bbd->wanted_co, vec);
692     }
693   }
694   bbd->wanted_speed = asbr->speed * val->max_speed;
695
696   return 1;
697 }
698 static int rule_fight(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa)
699 {
700   BoidRuleFight *fbr = (BoidRuleFight *)rule;
701   KDTreeNearest_3d *ptn = NULL;
702   ParticleTarget *pt;
703   ParticleData *epars;
704   ParticleData *enemy_pa = NULL;
705   BoidParticle *bpa;
706   /* friends & enemies */
707   float closest_enemy[3] = {0.0f, 0.0f, 0.0f};
708   float closest_dist = fbr->distance + 1.0f;
709   float f_strength = 0.0f, e_strength = 0.0f;
710   float health = 0.0f;
711   int n, ret = 0;
712
713   /* calculate own group strength */
714   int neighbors = BLI_kdtree_3d_range_search(
715       bbd->sim->psys->tree, pa->prev_state.co, &ptn, fbr->distance);
716   for (n = 0; n < neighbors; n++) {
717     bpa = bbd->sim->psys->particles[ptn[n].index].boid;
718     health += bpa->data.health;
719   }
720
721   f_strength += bbd->part->boids->strength * health;
722
723   if (ptn) {
724     MEM_freeN(ptn);
725     ptn = NULL;
726   }
727
728   /* add other friendlies and calculate enemy strength and find closest enemy */
729   for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) {
730     ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt);
731     if (epsys) {
732       epars = epsys->particles;
733
734       neighbors = BLI_kdtree_3d_range_search(epsys->tree, pa->prev_state.co, &ptn, fbr->distance);
735
736       health = 0.0f;
737
738       for (n = 0; n < neighbors; n++) {
739         bpa = epars[ptn[n].index].boid;
740         health += bpa->data.health;
741
742         if (n == 0 && pt->mode == PTARGET_MODE_ENEMY && ptn[n].dist < closest_dist) {
743           copy_v3_v3(closest_enemy, ptn[n].co);
744           closest_dist = ptn[n].dist;
745           enemy_pa = epars + ptn[n].index;
746         }
747       }
748       if (pt->mode == PTARGET_MODE_ENEMY) {
749         e_strength += epsys->part->boids->strength * health;
750       }
751       else if (pt->mode == PTARGET_MODE_FRIEND) {
752         f_strength += epsys->part->boids->strength * health;
753       }
754
755       if (ptn) {
756         MEM_freeN(ptn);
757         ptn = NULL;
758       }
759     }
760   }
761   /* decide action if enemy presence found */
762   if (e_strength > 0.0f) {
763     sub_v3_v3v3(bbd->wanted_co, closest_enemy, pa->prev_state.co);
764
765     /* attack if in range */
766     if (closest_dist <= bbd->part->boids->range + pa->size + enemy_pa->size) {
767       float damage = BLI_rng_get_float(bbd->rng);
768       float enemy_dir[3];
769
770       normalize_v3_v3(enemy_dir, bbd->wanted_co);
771
772       /* fight mode */
773       bbd->wanted_speed = 0.0f;
774
775       /* must face enemy to fight */
776       if (dot_v3v3(pa->prev_state.ave, enemy_dir) > 0.5f) {
777         bpa = enemy_pa->boid;
778         bpa->data.health -= bbd->part->boids->strength * bbd->timestep *
779                             ((1.0f - bbd->part->boids->accuracy) * damage +
780                              bbd->part->boids->accuracy);
781       }
782     }
783     else {
784       /* approach mode */
785       bbd->wanted_speed = val->max_speed;
786     }
787
788     /* check if boid doesn't want to fight */
789     bpa = pa->boid;
790     if (bpa->data.health / bbd->part->boids->health * bbd->part->boids->aggression <
791         e_strength / f_strength) {
792       /* decide to flee */
793       if (closest_dist < fbr->flee_distance * fbr->distance) {
794         negate_v3(bbd->wanted_co);
795         bbd->wanted_speed = val->max_speed;
796       }
797       else { /* wait for better odds */
798         bbd->wanted_speed = 0.0f;
799       }
800     }
801
802     ret = 1;
803   }
804
805   return ret;
806 }
807
808 typedef int (*boid_rule_cb)(BoidRule *rule,
809                             BoidBrainData *data,
810                             BoidValues *val,
811                             ParticleData *pa);
812
813 static boid_rule_cb boid_rules[] = {
814     rule_none,
815     rule_goal_avoid,
816     rule_goal_avoid,
817     rule_avoid_collision,
818     rule_separate,
819     rule_flock,
820     rule_follow_leader,
821     rule_average_speed,
822     rule_fight,
823     //rule_help,
824     //rule_protect,
825     //rule_hide,
826     //rule_follow_path,
827     //rule_follow_wall,
828 };
829
830 static void set_boid_values(BoidValues *val, BoidSettings *boids, ParticleData *pa)
831 {
832   BoidParticle *bpa = pa->boid;
833
834   if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) {
835     val->max_speed = boids->land_max_speed * bpa->data.health / boids->health;
836     val->max_acc = boids->land_max_acc * val->max_speed;
837     val->max_ave = boids->land_max_ave * (float)M_PI * bpa->data.health / boids->health;
838     val->min_speed = 0.0f; /* no minimum speed on land */
839     val->personal_space = boids->land_personal_space;
840     val->jump_speed = boids->land_jump_speed * bpa->data.health / boids->health;
841   }
842   else {
843     val->max_speed = boids->air_max_speed * bpa->data.health / boids->health;
844     val->max_acc = boids->air_max_acc * val->max_speed;
845     val->max_ave = boids->air_max_ave * (float)M_PI * bpa->data.health / boids->health;
846     val->min_speed = boids->air_min_speed * boids->air_max_speed;
847     val->personal_space = boids->air_personal_space;
848     val->jump_speed = 0.0f; /* no jumping in air */
849   }
850 }
851
852 static Object *boid_find_ground(BoidBrainData *bbd,
853                                 ParticleData *pa,
854                                 float ground_co[3],
855                                 float ground_nor[3])
856 {
857   const int raycast_flag = BVH_RAYCAST_DEFAULT & ~(BVH_RAYCAST_WATERTIGHT);
858   BoidParticle *bpa = pa->boid;
859
860   if (bpa->data.mode == eBoidMode_Climbing) {
861     SurfaceModifierData *surmd = NULL;
862     float x[3], v[3];
863
864     surmd = (SurfaceModifierData *)modifiers_findByType(bpa->ground, eModifierType_Surface);
865
866     /* take surface velocity into account */
867     closest_point_on_surface(surmd, pa->state.co, x, NULL, v);
868     add_v3_v3(x, v);
869
870     /* get actual position on surface */
871     closest_point_on_surface(surmd, x, ground_co, ground_nor, NULL);
872
873     return bpa->ground;
874   }
875   else {
876     float zvec[3] = {0.0f, 0.0f, 2000.0f};
877     ParticleCollision col;
878     ColliderCache *coll;
879     BVHTreeRayHit hit;
880     float radius = 0.0f, t, ray_dir[3];
881
882     if (!bbd->sim->colliders) {
883       return NULL;
884     }
885
886     memset(&col, 0, sizeof(ParticleCollision));
887
888     /* first try to find below boid */
889     copy_v3_v3(col.co1, pa->state.co);
890     sub_v3_v3v3(col.co2, pa->state.co, zvec);
891     sub_v3_v3v3(ray_dir, col.co2, col.co1);
892     col.f = 0.0f;
893     hit.index = -1;
894     hit.dist = col.original_ray_length = normalize_v3(ray_dir);
895     col.pce.inside = 0;
896
897     for (coll = bbd->sim->colliders->first; coll; coll = coll->next) {
898       col.current = coll->ob;
899       col.md = coll->collmd;
900       col.fac1 = col.fac2 = 0.f;
901
902       if (col.md && col.md->bvhtree) {
903         BLI_bvhtree_ray_cast_ex(col.md->bvhtree,
904                                 col.co1,
905                                 ray_dir,
906                                 radius,
907                                 &hit,
908                                 BKE_psys_collision_neartest_cb,
909                                 &col,
910                                 raycast_flag);
911       }
912     }
913     /* then use that object */
914     if (hit.index >= 0) {
915       t = hit.dist / col.original_ray_length;
916       interp_v3_v3v3(ground_co, col.co1, col.co2, t);
917       normalize_v3_v3(ground_nor, col.pce.nor);
918       return col.hit;
919     }
920
921     /* couldn't find below, so find upmost deflector object */
922     add_v3_v3v3(col.co1, pa->state.co, zvec);
923     sub_v3_v3v3(col.co2, pa->state.co, zvec);
924     sub_v3_v3(col.co2, zvec);
925     sub_v3_v3v3(ray_dir, col.co2, col.co1);
926     col.f = 0.0f;
927     hit.index = -1;
928     hit.dist = col.original_ray_length = normalize_v3(ray_dir);
929
930     for (coll = bbd->sim->colliders->first; coll; coll = coll->next) {
931       col.current = coll->ob;
932       col.md = coll->collmd;
933
934       if (col.md && col.md->bvhtree) {
935         BLI_bvhtree_ray_cast_ex(col.md->bvhtree,
936                                 col.co1,
937                                 ray_dir,
938                                 radius,
939                                 &hit,
940                                 BKE_psys_collision_neartest_cb,
941                                 &col,
942                                 raycast_flag);
943       }
944     }
945     /* then use that object */
946     if (hit.index >= 0) {
947       t = hit.dist / col.original_ray_length;
948       interp_v3_v3v3(ground_co, col.co1, col.co2, t);
949       normalize_v3_v3(ground_nor, col.pce.nor);
950       return col.hit;
951     }
952
953     /* default to z=0 */
954     copy_v3_v3(ground_co, pa->state.co);
955     ground_co[2] = 0;
956     ground_nor[0] = ground_nor[1] = 0.0f;
957     ground_nor[2] = 1.0f;
958     return NULL;
959   }
960 }
961 static int boid_rule_applies(ParticleData *pa, BoidSettings *UNUSED(boids), BoidRule *rule)
962 {
963   BoidParticle *bpa = pa->boid;
964
965   if (rule == NULL) {
966     return 0;
967   }
968
969   if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) &&
970       rule->flag & BOIDRULE_ON_LAND) {
971     return 1;
972   }
973
974   if (bpa->data.mode == eBoidMode_InAir && rule->flag & BOIDRULE_IN_AIR) {
975     return 1;
976   }
977
978   return 0;
979 }
980 void boids_precalc_rules(ParticleSettings *part, float cfra)
981 {
982   BoidState *state = part->boids->states.first;
983   BoidRule *rule;
984   for (; state; state = state->next) {
985     for (rule = state->rules.first; rule; rule = rule->next) {
986       if (rule->type == eBoidRuleType_FollowLeader) {
987         BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader *)rule;
988
989         if (flbr->ob && flbr->cfra != cfra) {
990           /* save object locations for velocity calculations */
991           copy_v3_v3(flbr->oloc, flbr->loc);
992           copy_v3_v3(flbr->loc, flbr->ob->obmat[3]);
993           flbr->cfra = cfra;
994         }
995       }
996     }
997   }
998 }
999 static void boid_climb(BoidSettings *boids,
1000                        ParticleData *pa,
1001                        float *surface_co,
1002                        float *surface_nor)
1003 {
1004   BoidParticle *bpa = pa->boid;
1005   float nor[3], vel[3];
1006   copy_v3_v3(nor, surface_nor);
1007
1008   /* gather apparent gravity */
1009   madd_v3_v3fl(bpa->gravity, surface_nor, -1.0f);
1010   normalize_v3(bpa->gravity);
1011
1012   /* raise boid it's size from surface */
1013   mul_v3_fl(nor, pa->size * boids->height);
1014   add_v3_v3v3(pa->state.co, surface_co, nor);
1015
1016   /* remove normal component from velocity */
1017   project_v3_v3v3(vel, pa->state.vel, surface_nor);
1018   sub_v3_v3v3(pa->state.vel, pa->state.vel, vel);
1019 }
1020 static float boid_goal_signed_dist(float *boid_co, float *goal_co, float *goal_nor)
1021 {
1022   float vec[3];
1023
1024   sub_v3_v3v3(vec, boid_co, goal_co);
1025
1026   return dot_v3v3(vec, goal_nor);
1027 }
1028 /* wanted_co is relative to boid location */
1029 static int apply_boid_rule(
1030     BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness)
1031 {
1032   if (rule == NULL) {
1033     return 0;
1034   }
1035
1036   if (boid_rule_applies(pa, bbd->part->boids, rule) == 0) {
1037     return 0;
1038   }
1039
1040   if (boid_rules[rule->type](rule, bbd, val, pa) == 0) {
1041     return 0;
1042   }
1043
1044   if (fuzziness < 0.0f || compare_len_v3v3(bbd->wanted_co,
1045                                            pa->prev_state.vel,
1046                                            fuzziness * len_v3(pa->prev_state.vel)) == 0) {
1047     return 1;
1048   }
1049   else {
1050     return 0;
1051   }
1052 }
1053 static BoidState *get_boid_state(BoidSettings *boids, ParticleData *pa)
1054 {
1055   BoidState *state = boids->states.first;
1056   BoidParticle *bpa = pa->boid;
1057
1058   for (; state; state = state->next) {
1059     if (state->id == bpa->data.state_id) {
1060       return state;
1061     }
1062   }
1063
1064   /* for some reason particle isn't at a valid state */
1065   state = boids->states.first;
1066   if (state) {
1067     bpa->data.state_id = state->id;
1068   }
1069
1070   return state;
1071 }
1072 //static int boid_condition_is_true(BoidCondition *cond)
1073 //{
1074 //  /* TODO */
1075 //  return 0;
1076 //}
1077
1078 /* determines the velocity the boid wants to have */
1079 void boid_brain(BoidBrainData *bbd, int p, ParticleData *pa)
1080 {
1081   BoidRule *rule;
1082   BoidSettings *boids = bbd->part->boids;
1083   BoidValues val;
1084   BoidState *state = get_boid_state(boids, pa);
1085   BoidParticle *bpa = pa->boid;
1086   ParticleSystem *psys = bbd->sim->psys;
1087   int rand;
1088   //BoidCondition *cond;
1089
1090   if (bpa->data.health <= 0.0f) {
1091     pa->alive = PARS_DYING;
1092     pa->dietime = bbd->cfra;
1093     return;
1094   }
1095
1096   //planned for near future
1097   //cond = state->conditions.first;
1098   //for (; cond; cond=cond->next) {
1099   //  if (boid_condition_is_true(cond)) {
1100   //      pa->boid->state_id = cond->state_id;
1101   //      state = get_boid_state(boids, pa);
1102   //      break; /* only first true condition is used */
1103   //  }
1104   //}
1105
1106   zero_v3(bbd->wanted_co);
1107   bbd->wanted_speed = 0.0f;
1108
1109   /* create random seed for every particle & frame */
1110   rand = (int)(psys_frand(psys, psys->seed + p) * 1000);
1111   rand = (int)(psys_frand(psys, (int)bbd->cfra + rand) * 1000);
1112
1113   set_boid_values(&val, bbd->part->boids, pa);
1114
1115   /* go through rules */
1116   switch (state->ruleset_type) {
1117     case eBoidRulesetType_Fuzzy: {
1118       for (rule = state->rules.first; rule; rule = rule->next) {
1119         if (apply_boid_rule(bbd, rule, &val, pa, state->rule_fuzziness)) {
1120           break; /* only first nonzero rule that comes through fuzzy rule is applied */
1121         }
1122       }
1123       break;
1124     }
1125     case eBoidRulesetType_Random: {
1126       /* use random rule for each particle (always same for same particle though) */
1127       const int n = BLI_listbase_count(&state->rules);
1128       if (n) {
1129         rule = BLI_findlink(&state->rules, rand % n);
1130         apply_boid_rule(bbd, rule, &val, pa, -1.0);
1131       }
1132       break;
1133     }
1134     case eBoidRulesetType_Average: {
1135       float wanted_co[3] = {0.0f, 0.0f, 0.0f}, wanted_speed = 0.0f;
1136       int n = 0;
1137       for (rule = state->rules.first; rule; rule = rule->next) {
1138         if (apply_boid_rule(bbd, rule, &val, pa, -1.0f)) {
1139           add_v3_v3(wanted_co, bbd->wanted_co);
1140           wanted_speed += bbd->wanted_speed;
1141           n++;
1142           zero_v3(bbd->wanted_co);
1143           bbd->wanted_speed = 0.0f;
1144         }
1145       }
1146
1147       if (n > 1) {
1148         mul_v3_fl(wanted_co, 1.0f / (float)n);
1149         wanted_speed /= (float)n;
1150       }
1151
1152       copy_v3_v3(bbd->wanted_co, wanted_co);
1153       bbd->wanted_speed = wanted_speed;
1154       break;
1155     }
1156   }
1157
1158   /* decide on jumping & liftoff */
1159   if (bpa->data.mode == eBoidMode_OnLand) {
1160     /* fuzziness makes boids capable of misjudgement */
1161     float mul = 1.0f + state->rule_fuzziness;
1162
1163     if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) {
1164       float cvel[3], dir[3];
1165
1166       copy_v3_v3(dir, pa->prev_state.ave);
1167       normalize_v2(dir);
1168
1169       copy_v3_v3(cvel, bbd->wanted_co);
1170       normalize_v2(cvel);
1171
1172       if (dot_v2v2(cvel, dir) > 0.95f / mul) {
1173         bpa->data.mode = eBoidMode_Liftoff;
1174       }
1175     }
1176     else if (val.jump_speed > 0.0f) {
1177       float jump_v[3];
1178       int jump = 0;
1179
1180       /* jump to get to a location */
1181       if (bbd->wanted_co[2] > 0.0f) {
1182         float cvel[3], dir[3];
1183         float z_v, ground_v, cur_v;
1184         float len;
1185
1186         copy_v3_v3(dir, pa->prev_state.ave);
1187         normalize_v2(dir);
1188
1189         copy_v3_v3(cvel, bbd->wanted_co);
1190         normalize_v2(cvel);
1191
1192         len = len_v2(pa->prev_state.vel);
1193
1194         /* first of all, are we going in a suitable direction? */
1195         /* or at a suitably slow speed */
1196         if (dot_v2v2(cvel, dir) > 0.95f / mul || len <= state->rule_fuzziness) {
1197           /* try to reach goal at highest point of the parabolic path */
1198           cur_v = len_v2(pa->prev_state.vel);
1199           z_v = sasqrt(-2.0f * bbd->sim->scene->physics_settings.gravity[2] * bbd->wanted_co[2]);
1200           ground_v = len_v2(bbd->wanted_co) *
1201                      sasqrt(-0.5f * bbd->sim->scene->physics_settings.gravity[2] /
1202                             bbd->wanted_co[2]);
1203
1204           len = sasqrt((ground_v - cur_v) * (ground_v - cur_v) + z_v * z_v);
1205
1206           if (len < val.jump_speed * mul || bbd->part->boids->options & BOID_ALLOW_FLIGHT) {
1207             jump = 1;
1208
1209             len = MIN2(len, val.jump_speed);
1210
1211             copy_v3_v3(jump_v, dir);
1212             jump_v[2] = z_v;
1213             mul_v3_fl(jump_v, ground_v);
1214
1215             normalize_v3(jump_v);
1216             mul_v3_fl(jump_v, len);
1217             add_v2_v2v2(jump_v, jump_v, pa->prev_state.vel);
1218           }
1219         }
1220       }
1221
1222       /* jump to go faster */
1223       if (jump == 0 && val.jump_speed > val.max_speed && bbd->wanted_speed > val.max_speed) {
1224         /* pass */
1225       }
1226
1227       if (jump) {
1228         copy_v3_v3(pa->prev_state.vel, jump_v);
1229         bpa->data.mode = eBoidMode_Falling;
1230       }
1231     }
1232   }
1233 }
1234 /* tries to realize the wanted velocity taking all constraints into account */
1235 void boid_body(BoidBrainData *bbd, ParticleData *pa)
1236 {
1237   BoidSettings *boids = bbd->part->boids;
1238   BoidParticle *bpa = pa->boid;
1239   BoidValues val;
1240   EffectedPoint epoint;
1241   float acc[3] = {0.0f, 0.0f, 0.0f}, tan_acc[3], nor_acc[3];
1242   float dvec[3], bvec[3];
1243   float new_dir[3], new_speed;
1244   float old_dir[3], old_speed;
1245   float wanted_dir[3];
1246   float q[4], mat[3][3]; /* rotation */
1247   float ground_co[3] = {0.0f, 0.0f, 0.0f}, ground_nor[3] = {0.0f, 0.0f, 1.0f};
1248   float force[3] = {0.0f, 0.0f, 0.0f};
1249   float pa_mass = bbd->part->mass, dtime = bbd->dfra * bbd->timestep;
1250
1251   set_boid_values(&val, boids, pa);
1252
1253   /* make sure there's something in new velocity, location & rotation */
1254   copy_particle_key(&pa->state, &pa->prev_state, 0);
1255
1256   if (bbd->part->flag & PART_SIZEMASS) {
1257     pa_mass *= pa->size;
1258   }
1259
1260   /* if boids can't fly they fall to the ground */
1261   if ((boids->options & BOID_ALLOW_FLIGHT) == 0 &&
1262       ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) == 0 &&
1263       psys_uses_gravity(bbd->sim)) {
1264     bpa->data.mode = eBoidMode_Falling;
1265   }
1266
1267   if (bpa->data.mode == eBoidMode_Falling) {
1268     /* Falling boids are only effected by gravity. */
1269     acc[2] = bbd->sim->scene->physics_settings.gravity[2];
1270   }
1271   else {
1272     /* figure out acceleration */
1273     float landing_level = 2.0f;
1274     float level = landing_level + 1.0f;
1275     float new_vel[3];
1276
1277     if (bpa->data.mode == eBoidMode_Liftoff) {
1278       bpa->data.mode = eBoidMode_InAir;
1279       bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor);
1280     }
1281     else if (bpa->data.mode == eBoidMode_InAir && boids->options & BOID_ALLOW_LAND) {
1282       /* auto-leveling & landing if close to ground */
1283
1284       bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor);
1285
1286       /* level = how many particle sizes above ground */
1287       level = (pa->prev_state.co[2] - ground_co[2]) / (2.0f * pa->size) - 0.5f;
1288
1289       landing_level = -boids->landing_smoothness * pa->prev_state.vel[2] * pa_mass;
1290
1291       if (pa->prev_state.vel[2] < 0.0f) {
1292         if (level < 1.0f) {
1293           bbd->wanted_co[0] = bbd->wanted_co[1] = bbd->wanted_co[2] = 0.0f;
1294           bbd->wanted_speed = 0.0f;
1295           bpa->data.mode = eBoidMode_Falling;
1296         }
1297         else if (level < landing_level) {
1298           bbd->wanted_speed *= (level - 1.0f) / landing_level;
1299           bbd->wanted_co[2] *= (level - 1.0f) / landing_level;
1300         }
1301       }
1302     }
1303
1304     copy_v3_v3(old_dir, pa->prev_state.ave);
1305     new_speed = normalize_v3_v3(wanted_dir, bbd->wanted_co);
1306
1307     /* first check if we have valid direction we want to go towards */
1308     if (new_speed == 0.0f) {
1309       copy_v3_v3(new_dir, old_dir);
1310     }
1311     else {
1312       float old_dir2[2], wanted_dir2[2], nor[3], angle;
1313       copy_v2_v2(old_dir2, old_dir);
1314       normalize_v2(old_dir2);
1315       copy_v2_v2(wanted_dir2, wanted_dir);
1316       normalize_v2(wanted_dir2);
1317
1318       /* choose random direction to turn if wanted velocity */
1319       /* is directly behind regardless of z-coordinate */
1320       if (dot_v2v2(old_dir2, wanted_dir2) < -0.99f) {
1321         wanted_dir[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1322         wanted_dir[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1323         wanted_dir[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng));
1324         normalize_v3(wanted_dir);
1325       }
1326
1327       /* constrain direction with maximum angular velocity */
1328       angle = saacos(dot_v3v3(old_dir, wanted_dir));
1329       angle = min_ff(angle, val.max_ave);
1330
1331       cross_v3_v3v3(nor, old_dir, wanted_dir);
1332       axis_angle_to_quat(q, nor, angle);
1333       copy_v3_v3(new_dir, old_dir);
1334       mul_qt_v3(q, new_dir);
1335       normalize_v3(new_dir);
1336
1337       /* save direction in case resulting velocity too small */
1338       axis_angle_to_quat(q, nor, angle * dtime);
1339       copy_v3_v3(pa->state.ave, old_dir);
1340       mul_qt_v3(q, pa->state.ave);
1341       normalize_v3(pa->state.ave);
1342     }
1343
1344     /* constrain speed with maximum acceleration */
1345     old_speed = len_v3(pa->prev_state.vel);
1346
1347     if (bbd->wanted_speed < old_speed) {
1348       new_speed = MAX2(bbd->wanted_speed, old_speed - val.max_acc);
1349     }
1350     else {
1351       new_speed = MIN2(bbd->wanted_speed, old_speed + val.max_acc);
1352     }
1353
1354     /* combine direction and speed */
1355     copy_v3_v3(new_vel, new_dir);
1356     mul_v3_fl(new_vel, new_speed);
1357
1358     /* maintain minimum flying velocity if not landing */
1359     if (level >= landing_level) {
1360       float len2 = dot_v2v2(new_vel, new_vel);
1361       float root;
1362
1363       len2 = MAX2(len2, val.min_speed * val.min_speed);
1364       root = sasqrt(new_speed * new_speed - len2);
1365
1366       new_vel[2] = new_vel[2] < 0.0f ? -root : root;
1367
1368       normalize_v2(new_vel);
1369       mul_v2_fl(new_vel, sasqrt(len2));
1370     }
1371
1372     /* finally constrain speed to max speed */
1373     new_speed = normalize_v3(new_vel);
1374     mul_v3_fl(new_vel, MIN2(new_speed, val.max_speed));
1375
1376     /* get acceleration from difference of velocities */
1377     sub_v3_v3v3(acc, new_vel, pa->prev_state.vel);
1378
1379     /* break acceleration to components */
1380     project_v3_v3v3(tan_acc, acc, pa->prev_state.ave);
1381     sub_v3_v3v3(nor_acc, acc, tan_acc);
1382   }
1383
1384   /* account for effectors */
1385   pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint);
1386   BKE_effectors_apply(bbd->sim->psys->effectors,
1387                       bbd->sim->colliders,
1388                       bbd->part->effector_weights,
1389                       &epoint,
1390                       force,
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
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 }
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
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
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 }
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 }
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 }
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 }