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