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