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