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