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