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