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