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