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