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