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