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