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