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