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