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