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