diff options
Diffstat (limited to 'source/blender/blenkernel/intern/boids.c')
-rw-r--r-- | source/blender/blenkernel/intern/boids.c | 3002 |
1 files changed, 1542 insertions, 1460 deletions
diff --git a/source/blender/blenkernel/intern/boids.c b/source/blender/blenkernel/intern/boids.c index c092b2c7e79..b31a283448c 100644 --- a/source/blender/blenkernel/intern/boids.c +++ b/source/blender/blenkernel/intern/boids.c @@ -21,7 +21,6 @@ * \ingroup bke */ - #include <string.h> #include <math.h> @@ -45,1587 +44,1670 @@ #include "RNA_enum_types.h" -static float len_squared_v3v3_with_normal_bias( - const float co_search[3], const float co_test[3], const void *user_data) +static float len_squared_v3v3_with_normal_bias(const float co_search[3], + const float co_test[3], + const void *user_data) { - const float *normal = user_data; - float d[3], dist; + const float *normal = user_data; + float d[3], dist; - sub_v3_v3v3(d, co_test, co_search); + sub_v3_v3v3(d, co_test, co_search); - dist = len_squared_v3(d); + dist = len_squared_v3(d); - /* Avoid head-on collisions. */ - if (dot_v3v3(d, normal) < 0.0f) { - dist *= 10.0f; - } - return dist; + /* Avoid head-on collisions. */ + if (dot_v3v3(d, normal) < 0.0f) { + dist *= 10.0f; + } + return dist; } typedef struct BoidValues { - float max_speed, max_acc; - float max_ave, min_speed; - float personal_space, jump_speed; + float max_speed, max_acc; + float max_ave, min_speed; + float personal_space, jump_speed; } BoidValues; -static int apply_boid_rule(BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness); +static int apply_boid_rule( + BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness); -static int rule_none(BoidRule *UNUSED(rule), BoidBrainData *UNUSED(data), BoidValues *UNUSED(val), ParticleData *UNUSED(pa)) +static int rule_none(BoidRule *UNUSED(rule), + BoidBrainData *UNUSED(data), + BoidValues *UNUSED(val), + ParticleData *UNUSED(pa)) { - return 0; + return 0; } static int rule_goal_avoid(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa) { - BoidRuleGoalAvoid *gabr = (BoidRuleGoalAvoid*) rule; - BoidSettings *boids = bbd->part->boids; - BoidParticle *bpa = pa->boid; - EffectedPoint epoint; - ListBase *effectors = bbd->sim->psys->effectors; - EffectorCache *cur, *eff = NULL; - EffectorCache temp_eff; - EffectorData efd, cur_efd; - float mul = (rule->type == eBoidRuleType_Avoid ? 1.0 : -1.0); - float priority = 0.0f, len = 0.0f; - int ret = 0; - - int p = 0; - efd.index = cur_efd.index = &p; - - pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint); - - /* first find out goal/predator with highest priority */ - if (effectors) for (cur = effectors->first; cur; cur=cur->next) { - Object *eob = cur->ob; - PartDeflect *pd = cur->pd; - - if (gabr->ob && (rule->type != eBoidRuleType_Goal || gabr->ob != bpa->ground)) { - if (gabr->ob == eob) { - /* TODO: effectors with multiple points */ - if (get_effector_data(cur, &efd, &epoint, 0)) { - if (cur->pd && cur->pd->forcefield == PFIELD_BOID) - priority = mul * pd->f_strength * effector_falloff(cur, &efd, &epoint, bbd->part->effector_weights); - else - priority = 1.0; - - eff = cur; - } - break; - } - } - else if (rule->type == eBoidRuleType_Goal && eob == bpa->ground) { - /* skip current object */ - } - else if (pd->forcefield == PFIELD_BOID && mul * pd->f_strength > 0.0f && get_effector_data(cur, &cur_efd, &epoint, 0)) { - float temp = mul * pd->f_strength * effector_falloff(cur, &cur_efd, &epoint, bbd->part->effector_weights); - - if (temp == 0.0f) { - /* do nothing */ - } - else if (temp > priority) { - priority = temp; - eff = cur; - efd = cur_efd; - len = efd.distance; - } - /* choose closest object with same priority */ - else if (temp == priority && efd.distance < len) { - eff = cur; - efd = cur_efd; - len = efd.distance; - } - } - } - - /* if the object doesn't have effector data we have to fake it */ - if (eff == NULL && gabr->ob) { - memset(&temp_eff, 0, sizeof(EffectorCache)); - temp_eff.ob = gabr->ob; - temp_eff.depsgraph = bbd->sim->depsgraph; - temp_eff.scene = bbd->sim->scene; - eff = &temp_eff; - get_effector_data(eff, &efd, &epoint, 0); - priority = 1.0f; - } - - /* then use that effector */ - if (priority > (rule->type==eBoidRuleType_Avoid ? gabr->fear_factor : 0.0f)) { /* with avoid, factor is "fear factor" */ - Object *eob = eff->ob; - PartDeflect *pd = eff->pd; - float surface = (pd && pd->shape == PFIELD_SHAPE_SURFACE) ? 1.0f : 0.0f; - - if (gabr->options & BRULE_GOAL_AVOID_PREDICT) { - /* estimate future location of target */ - get_effector_data(eff, &efd, &epoint, 1); - - mul_v3_fl(efd.vel, efd.distance / (val->max_speed * bbd->timestep)); - add_v3_v3(efd.loc, efd.vel); - sub_v3_v3v3(efd.vec_to_point, pa->prev_state.co, efd.loc); - efd.distance = len_v3(efd.vec_to_point); - } - - if (rule->type == eBoidRuleType_Goal && boids->options & BOID_ALLOW_CLIMB && surface!=0.0f) { - if (!bbd->goal_ob || bbd->goal_priority < priority) { - bbd->goal_ob = eob; - copy_v3_v3(bbd->goal_co, efd.loc); - copy_v3_v3(bbd->goal_nor, efd.nor); - } - } - else if ((rule->type == eBoidRuleType_Avoid) && - (bpa->data.mode == eBoidMode_Climbing) && - (priority > 2.0f * gabr->fear_factor)) - { - /* detach from surface and try to fly away from danger */ - negate_v3_v3(efd.vec_to_point, bpa->gravity); - } - - copy_v3_v3(bbd->wanted_co, efd.vec_to_point); - mul_v3_fl(bbd->wanted_co, mul); - - bbd->wanted_speed = val->max_speed * priority; - - /* with goals factor is approach velocity factor */ - if (rule->type == eBoidRuleType_Goal && boids->landing_smoothness > 0.0f) { - float len2 = 2.0f*len_v3(pa->prev_state.vel); - - surface *= pa->size * boids->height; - - if (len2 > 0.0f && efd.distance - surface < len2) { - len2 = (efd.distance - surface)/len2; - bbd->wanted_speed *= powf(len2, boids->landing_smoothness); - } - } - - ret = 1; - } - - return ret; + BoidRuleGoalAvoid *gabr = (BoidRuleGoalAvoid *)rule; + BoidSettings *boids = bbd->part->boids; + BoidParticle *bpa = pa->boid; + EffectedPoint epoint; + ListBase *effectors = bbd->sim->psys->effectors; + EffectorCache *cur, *eff = NULL; + EffectorCache temp_eff; + EffectorData efd, cur_efd; + float mul = (rule->type == eBoidRuleType_Avoid ? 1.0 : -1.0); + float priority = 0.0f, len = 0.0f; + int ret = 0; + + int p = 0; + efd.index = cur_efd.index = &p; + + pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint); + + /* first find out goal/predator with highest priority */ + if (effectors) + for (cur = effectors->first; cur; cur = cur->next) { + Object *eob = cur->ob; + PartDeflect *pd = cur->pd; + + if (gabr->ob && (rule->type != eBoidRuleType_Goal || gabr->ob != bpa->ground)) { + if (gabr->ob == eob) { + /* TODO: effectors with multiple points */ + if (get_effector_data(cur, &efd, &epoint, 0)) { + if (cur->pd && cur->pd->forcefield == PFIELD_BOID) + priority = mul * pd->f_strength * + effector_falloff(cur, &efd, &epoint, bbd->part->effector_weights); + else + priority = 1.0; + + eff = cur; + } + break; + } + } + else if (rule->type == eBoidRuleType_Goal && eob == bpa->ground) { + /* skip current object */ + } + else if (pd->forcefield == PFIELD_BOID && mul * pd->f_strength > 0.0f && + get_effector_data(cur, &cur_efd, &epoint, 0)) { + float temp = mul * pd->f_strength * + effector_falloff(cur, &cur_efd, &epoint, bbd->part->effector_weights); + + if (temp == 0.0f) { + /* do nothing */ + } + else if (temp > priority) { + priority = temp; + eff = cur; + efd = cur_efd; + len = efd.distance; + } + /* choose closest object with same priority */ + else if (temp == priority && efd.distance < len) { + eff = cur; + efd = cur_efd; + len = efd.distance; + } + } + } + + /* if the object doesn't have effector data we have to fake it */ + if (eff == NULL && gabr->ob) { + memset(&temp_eff, 0, sizeof(EffectorCache)); + temp_eff.ob = gabr->ob; + temp_eff.depsgraph = bbd->sim->depsgraph; + temp_eff.scene = bbd->sim->scene; + eff = &temp_eff; + get_effector_data(eff, &efd, &epoint, 0); + priority = 1.0f; + } + + /* then use that effector */ + if (priority > (rule->type == eBoidRuleType_Avoid ? + gabr->fear_factor : + 0.0f)) { /* with avoid, factor is "fear factor" */ + Object *eob = eff->ob; + PartDeflect *pd = eff->pd; + float surface = (pd && pd->shape == PFIELD_SHAPE_SURFACE) ? 1.0f : 0.0f; + + if (gabr->options & BRULE_GOAL_AVOID_PREDICT) { + /* estimate future location of target */ + get_effector_data(eff, &efd, &epoint, 1); + + mul_v3_fl(efd.vel, efd.distance / (val->max_speed * bbd->timestep)); + add_v3_v3(efd.loc, efd.vel); + sub_v3_v3v3(efd.vec_to_point, pa->prev_state.co, efd.loc); + efd.distance = len_v3(efd.vec_to_point); + } + + if (rule->type == eBoidRuleType_Goal && boids->options & BOID_ALLOW_CLIMB && surface != 0.0f) { + if (!bbd->goal_ob || bbd->goal_priority < priority) { + bbd->goal_ob = eob; + copy_v3_v3(bbd->goal_co, efd.loc); + copy_v3_v3(bbd->goal_nor, efd.nor); + } + } + else if ((rule->type == eBoidRuleType_Avoid) && (bpa->data.mode == eBoidMode_Climbing) && + (priority > 2.0f * gabr->fear_factor)) { + /* detach from surface and try to fly away from danger */ + negate_v3_v3(efd.vec_to_point, bpa->gravity); + } + + copy_v3_v3(bbd->wanted_co, efd.vec_to_point); + mul_v3_fl(bbd->wanted_co, mul); + + bbd->wanted_speed = val->max_speed * priority; + + /* with goals factor is approach velocity factor */ + if (rule->type == eBoidRuleType_Goal && boids->landing_smoothness > 0.0f) { + float len2 = 2.0f * len_v3(pa->prev_state.vel); + + surface *= pa->size * boids->height; + + if (len2 > 0.0f && efd.distance - surface < len2) { + len2 = (efd.distance - surface) / len2; + bbd->wanted_speed *= powf(len2, boids->landing_smoothness); + } + } + + ret = 1; + } + + return ret; } -static int rule_avoid_collision(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa) +static int rule_avoid_collision(BoidRule *rule, + BoidBrainData *bbd, + BoidValues *val, + ParticleData *pa) { - const int raycast_flag = BVH_RAYCAST_DEFAULT & ~(BVH_RAYCAST_WATERTIGHT); - BoidRuleAvoidCollision *acbr = (BoidRuleAvoidCollision*) rule; - KDTreeNearest_3d *ptn = NULL; - ParticleTarget *pt; - BoidParticle *bpa = pa->boid; - ColliderCache *coll; - float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; - float co1[3], vel1[3], co2[3], vel2[3]; - float len, t, inp, t_min = 2.0f; - int n, neighbors = 0, nearest = 0; - int ret = 0; - - //check deflector objects first - if (acbr->options & BRULE_ACOLL_WITH_DEFLECTORS && bbd->sim->colliders) { - ParticleCollision col; - BVHTreeRayHit hit; - float radius = val->personal_space * pa->size, ray_dir[3]; - - memset(&col, 0, sizeof(ParticleCollision)); - - copy_v3_v3(col.co1, pa->prev_state.co); - add_v3_v3v3(col.co2, pa->prev_state.co, pa->prev_state.vel); - sub_v3_v3v3(ray_dir, col.co2, col.co1); - mul_v3_fl(ray_dir, acbr->look_ahead); - col.f = 0.0f; - hit.index = -1; - hit.dist = col.original_ray_length = normalize_v3(ray_dir); - - /* find out closest deflector object */ - for (coll = bbd->sim->colliders->first; coll; coll=coll->next) { - /* don't check with current ground object */ - if (coll->ob == bpa->ground) - continue; - - col.current = coll->ob; - col.md = coll->collmd; - - if (col.md && col.md->bvhtree) { - BLI_bvhtree_ray_cast_ex( - col.md->bvhtree, col.co1, ray_dir, radius, &hit, - BKE_psys_collision_neartest_cb, &col, raycast_flag); - } - } - /* then avoid that object */ - if (hit.index>=0) { - t = hit.dist/col.original_ray_length; - - /* avoid head-on collision */ - if (dot_v3v3(col.pce.nor, pa->prev_state.ave) < -0.99f) { - /* don't know why, but uneven range [0.0, 1.0] */ - /* works much better than even [-1.0, 1.0] */ - bbd->wanted_co[0] = BLI_rng_get_float(bbd->rng); - bbd->wanted_co[1] = BLI_rng_get_float(bbd->rng); - bbd->wanted_co[2] = BLI_rng_get_float(bbd->rng); - } - else { - copy_v3_v3(bbd->wanted_co, col.pce.nor); - } - - mul_v3_fl(bbd->wanted_co, (1.0f - t) * val->personal_space * pa->size); - - bbd->wanted_speed = sqrtf(t) * len_v3(pa->prev_state.vel); - bbd->wanted_speed = MAX2(bbd->wanted_speed, val->min_speed); - - return 1; - } - } - - //check boids in own system - if (acbr->options & BRULE_ACOLL_WITH_BOIDS) { - neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb( - bbd->sim->psys->tree, pa->prev_state.co, &ptn, acbr->look_ahead * len_v3(pa->prev_state.vel), - len_squared_v3v3_with_normal_bias, pa->prev_state.ave); - if (neighbors > 1) for (n=1; n<neighbors; n++) { - copy_v3_v3(co1, pa->prev_state.co); - copy_v3_v3(vel1, pa->prev_state.vel); - copy_v3_v3(co2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.co); - copy_v3_v3(vel2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.vel); - - sub_v3_v3v3(loc, co1, co2); - - sub_v3_v3v3(vec, vel1, vel2); - - inp = dot_v3v3(vec, vec); - - /* velocities not parallel */ - if (inp != 0.0f) { - t = -dot_v3v3(loc, vec)/inp; - /* cpa is not too far in the future so investigate further */ - if (t > 0.0f && t < t_min) { - madd_v3_v3fl(co1, vel1, t); - madd_v3_v3fl(co2, vel2, t); - - sub_v3_v3v3(vec, co2, co1); - - len = normalize_v3(vec); - - /* distance of cpa is close enough */ - if (len < 2.0f * val->personal_space * pa->size) { - t_min = t; - - mul_v3_fl(vec, len_v3(vel1)); - mul_v3_fl(vec, (2.0f - t)/2.0f); - sub_v3_v3v3(bbd->wanted_co, vel1, vec); - bbd->wanted_speed = len_v3(bbd->wanted_co); - ret = 1; - } - } - } - } - } - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - - /* check boids in other systems */ - for (pt=bbd->sim->psys->targets.first; pt; pt=pt->next) { - ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); - - if (epsys) { - BLI_assert(epsys->tree != NULL); - neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb( - epsys->tree, pa->prev_state.co, &ptn, acbr->look_ahead * len_v3(pa->prev_state.vel), - len_squared_v3v3_with_normal_bias, pa->prev_state.ave); - - if (neighbors > 0) for (n=0; n<neighbors; n++) { - copy_v3_v3(co1, pa->prev_state.co); - copy_v3_v3(vel1, pa->prev_state.vel); - copy_v3_v3(co2, (epsys->particles + ptn[n].index)->prev_state.co); - copy_v3_v3(vel2, (epsys->particles + ptn[n].index)->prev_state.vel); - - sub_v3_v3v3(loc, co1, co2); - - sub_v3_v3v3(vec, vel1, vel2); - - inp = dot_v3v3(vec, vec); - - /* velocities not parallel */ - if (inp != 0.0f) { - t = -dot_v3v3(loc, vec)/inp; - /* cpa is not too far in the future so investigate further */ - if (t > 0.0f && t < t_min) { - madd_v3_v3fl(co1, vel1, t); - madd_v3_v3fl(co2, vel2, t); - - sub_v3_v3v3(vec, co2, co1); - - len = normalize_v3(vec); - - /* distance of cpa is close enough */ - if (len < 2.0f * val->personal_space * pa->size) { - t_min = t; - - mul_v3_fl(vec, len_v3(vel1)); - mul_v3_fl(vec, (2.0f - t)/2.0f); - sub_v3_v3v3(bbd->wanted_co, vel1, vec); - bbd->wanted_speed = len_v3(bbd->wanted_co); - ret = 1; - } - } - } - } - - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - } - } - - - if (ptn && nearest==0) - MEM_freeN(ptn); - - return ret; + const int raycast_flag = BVH_RAYCAST_DEFAULT & ~(BVH_RAYCAST_WATERTIGHT); + BoidRuleAvoidCollision *acbr = (BoidRuleAvoidCollision *)rule; + KDTreeNearest_3d *ptn = NULL; + ParticleTarget *pt; + BoidParticle *bpa = pa->boid; + ColliderCache *coll; + float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; + float co1[3], vel1[3], co2[3], vel2[3]; + float len, t, inp, t_min = 2.0f; + int n, neighbors = 0, nearest = 0; + int ret = 0; + + //check deflector objects first + if (acbr->options & BRULE_ACOLL_WITH_DEFLECTORS && bbd->sim->colliders) { + ParticleCollision col; + BVHTreeRayHit hit; + float radius = val->personal_space * pa->size, ray_dir[3]; + + memset(&col, 0, sizeof(ParticleCollision)); + + copy_v3_v3(col.co1, pa->prev_state.co); + add_v3_v3v3(col.co2, pa->prev_state.co, pa->prev_state.vel); + sub_v3_v3v3(ray_dir, col.co2, col.co1); + mul_v3_fl(ray_dir, acbr->look_ahead); + col.f = 0.0f; + hit.index = -1; + hit.dist = col.original_ray_length = normalize_v3(ray_dir); + + /* find out closest deflector object */ + for (coll = bbd->sim->colliders->first; coll; coll = coll->next) { + /* don't check with current ground object */ + if (coll->ob == bpa->ground) + continue; + + col.current = coll->ob; + col.md = coll->collmd; + + if (col.md && col.md->bvhtree) { + BLI_bvhtree_ray_cast_ex(col.md->bvhtree, + col.co1, + ray_dir, + radius, + &hit, + BKE_psys_collision_neartest_cb, + &col, + raycast_flag); + } + } + /* then avoid that object */ + if (hit.index >= 0) { + t = hit.dist / col.original_ray_length; + + /* avoid head-on collision */ + if (dot_v3v3(col.pce.nor, pa->prev_state.ave) < -0.99f) { + /* don't know why, but uneven range [0.0, 1.0] */ + /* works much better than even [-1.0, 1.0] */ + bbd->wanted_co[0] = BLI_rng_get_float(bbd->rng); + bbd->wanted_co[1] = BLI_rng_get_float(bbd->rng); + bbd->wanted_co[2] = BLI_rng_get_float(bbd->rng); + } + else { + copy_v3_v3(bbd->wanted_co, col.pce.nor); + } + + mul_v3_fl(bbd->wanted_co, (1.0f - t) * val->personal_space * pa->size); + + bbd->wanted_speed = sqrtf(t) * len_v3(pa->prev_state.vel); + bbd->wanted_speed = MAX2(bbd->wanted_speed, val->min_speed); + + return 1; + } + } + + //check boids in own system + if (acbr->options & BRULE_ACOLL_WITH_BOIDS) { + neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb(bbd->sim->psys->tree, + pa->prev_state.co, + &ptn, + acbr->look_ahead * + len_v3(pa->prev_state.vel), + len_squared_v3v3_with_normal_bias, + pa->prev_state.ave); + if (neighbors > 1) + for (n = 1; n < neighbors; n++) { + copy_v3_v3(co1, pa->prev_state.co); + copy_v3_v3(vel1, pa->prev_state.vel); + copy_v3_v3(co2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.co); + copy_v3_v3(vel2, (bbd->sim->psys->particles + ptn[n].index)->prev_state.vel); + + sub_v3_v3v3(loc, co1, co2); + + sub_v3_v3v3(vec, vel1, vel2); + + inp = dot_v3v3(vec, vec); + + /* velocities not parallel */ + if (inp != 0.0f) { + t = -dot_v3v3(loc, vec) / inp; + /* cpa is not too far in the future so investigate further */ + if (t > 0.0f && t < t_min) { + madd_v3_v3fl(co1, vel1, t); + madd_v3_v3fl(co2, vel2, t); + + sub_v3_v3v3(vec, co2, co1); + + len = normalize_v3(vec); + + /* distance of cpa is close enough */ + if (len < 2.0f * val->personal_space * pa->size) { + t_min = t; + + mul_v3_fl(vec, len_v3(vel1)); + mul_v3_fl(vec, (2.0f - t) / 2.0f); + sub_v3_v3v3(bbd->wanted_co, vel1, vec); + bbd->wanted_speed = len_v3(bbd->wanted_co); + ret = 1; + } + } + } + } + } + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + + /* check boids in other systems */ + for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) { + ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); + + if (epsys) { + BLI_assert(epsys->tree != NULL); + neighbors = BLI_kdtree_3d_range_search_with_len_squared_cb(epsys->tree, + pa->prev_state.co, + &ptn, + acbr->look_ahead * + len_v3(pa->prev_state.vel), + len_squared_v3v3_with_normal_bias, + pa->prev_state.ave); + + if (neighbors > 0) + for (n = 0; n < neighbors; n++) { + copy_v3_v3(co1, pa->prev_state.co); + copy_v3_v3(vel1, pa->prev_state.vel); + copy_v3_v3(co2, (epsys->particles + ptn[n].index)->prev_state.co); + copy_v3_v3(vel2, (epsys->particles + ptn[n].index)->prev_state.vel); + + sub_v3_v3v3(loc, co1, co2); + + sub_v3_v3v3(vec, vel1, vel2); + + inp = dot_v3v3(vec, vec); + + /* velocities not parallel */ + if (inp != 0.0f) { + t = -dot_v3v3(loc, vec) / inp; + /* cpa is not too far in the future so investigate further */ + if (t > 0.0f && t < t_min) { + madd_v3_v3fl(co1, vel1, t); + madd_v3_v3fl(co2, vel2, t); + + sub_v3_v3v3(vec, co2, co1); + + len = normalize_v3(vec); + + /* distance of cpa is close enough */ + if (len < 2.0f * val->personal_space * pa->size) { + t_min = t; + + mul_v3_fl(vec, len_v3(vel1)); + mul_v3_fl(vec, (2.0f - t) / 2.0f); + sub_v3_v3v3(bbd->wanted_co, vel1, vec); + bbd->wanted_speed = len_v3(bbd->wanted_co); + ret = 1; + } + } + } + } + + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + } + } + + if (ptn && nearest == 0) + MEM_freeN(ptn); + + return ret; } -static int rule_separate(BoidRule *UNUSED(rule), BoidBrainData *bbd, BoidValues *val, ParticleData *pa) +static int rule_separate(BoidRule *UNUSED(rule), + BoidBrainData *bbd, + BoidValues *val, + ParticleData *pa) { - KDTreeNearest_3d *ptn = NULL; - ParticleTarget *pt; - float len = 2.0f * val->personal_space * pa->size + 1.0f; - float vec[3] = {0.0f, 0.0f, 0.0f}; - int neighbors = BLI_kdtree_3d_range_search( - bbd->sim->psys->tree, pa->prev_state.co, - &ptn, 2.0f * val->personal_space * pa->size); - int ret = 0; - - if (neighbors > 1 && ptn[1].dist!=0.0f) { - sub_v3_v3v3(vec, pa->prev_state.co, bbd->sim->psys->particles[ptn[1].index].state.co); - mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[1].dist) / ptn[1].dist); - add_v3_v3(bbd->wanted_co, vec); - bbd->wanted_speed = val->max_speed; - len = ptn[1].dist; - ret = 1; - } - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - - /* check other boid systems */ - for (pt=bbd->sim->psys->targets.first; pt; pt=pt->next) { - ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); - - if (epsys) { - neighbors = BLI_kdtree_3d_range_search( - epsys->tree, pa->prev_state.co, - &ptn, 2.0f * val->personal_space * pa->size); - - if (neighbors > 0 && ptn[0].dist < len) { - sub_v3_v3v3(vec, pa->prev_state.co, ptn[0].co); - mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[0].dist) / ptn[1].dist); - add_v3_v3(bbd->wanted_co, vec); - bbd->wanted_speed = val->max_speed; - len = ptn[0].dist; - ret = 1; - } - - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - } - } - return ret; + KDTreeNearest_3d *ptn = NULL; + ParticleTarget *pt; + float len = 2.0f * val->personal_space * pa->size + 1.0f; + float vec[3] = {0.0f, 0.0f, 0.0f}; + int neighbors = BLI_kdtree_3d_range_search( + bbd->sim->psys->tree, pa->prev_state.co, &ptn, 2.0f * val->personal_space * pa->size); + int ret = 0; + + if (neighbors > 1 && ptn[1].dist != 0.0f) { + sub_v3_v3v3(vec, pa->prev_state.co, bbd->sim->psys->particles[ptn[1].index].state.co); + mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[1].dist) / ptn[1].dist); + add_v3_v3(bbd->wanted_co, vec); + bbd->wanted_speed = val->max_speed; + len = ptn[1].dist; + ret = 1; + } + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + + /* check other boid systems */ + for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) { + ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); + + if (epsys) { + neighbors = BLI_kdtree_3d_range_search( + epsys->tree, pa->prev_state.co, &ptn, 2.0f * val->personal_space * pa->size); + + if (neighbors > 0 && ptn[0].dist < len) { + sub_v3_v3v3(vec, pa->prev_state.co, ptn[0].co); + mul_v3_fl(vec, (2.0f * val->personal_space * pa->size - ptn[0].dist) / ptn[1].dist); + add_v3_v3(bbd->wanted_co, vec); + bbd->wanted_speed = val->max_speed; + len = ptn[0].dist; + ret = 1; + } + + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + } + } + return ret; } -static int rule_flock(BoidRule *UNUSED(rule), BoidBrainData *bbd, BoidValues *UNUSED(val), ParticleData *pa) +static int rule_flock(BoidRule *UNUSED(rule), + BoidBrainData *bbd, + BoidValues *UNUSED(val), + ParticleData *pa) { - KDTreeNearest_3d ptn[11]; - float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; - int neighbors = BLI_kdtree_3d_find_nearest_n_with_len_squared_cb( - bbd->sim->psys->tree, pa->state.co, ptn, ARRAY_SIZE(ptn), - len_squared_v3v3_with_normal_bias, pa->prev_state.ave); - int n; - int ret = 0; - - if (neighbors > 1) { - for (n=1; n<neighbors; n++) { - add_v3_v3(loc, bbd->sim->psys->particles[ptn[n].index].prev_state.co); - add_v3_v3(vec, bbd->sim->psys->particles[ptn[n].index].prev_state.vel); - } - - mul_v3_fl(loc, 1.0f/((float)neighbors - 1.0f)); - mul_v3_fl(vec, 1.0f/((float)neighbors - 1.0f)); - - sub_v3_v3(loc, pa->prev_state.co); - sub_v3_v3(vec, pa->prev_state.vel); - - add_v3_v3(bbd->wanted_co, vec); - add_v3_v3(bbd->wanted_co, loc); - bbd->wanted_speed = len_v3(bbd->wanted_co); - - ret = 1; - } - return ret; + KDTreeNearest_3d ptn[11]; + float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; + int neighbors = BLI_kdtree_3d_find_nearest_n_with_len_squared_cb( + bbd->sim->psys->tree, + pa->state.co, + ptn, + ARRAY_SIZE(ptn), + len_squared_v3v3_with_normal_bias, + pa->prev_state.ave); + int n; + int ret = 0; + + if (neighbors > 1) { + for (n = 1; n < neighbors; n++) { + add_v3_v3(loc, bbd->sim->psys->particles[ptn[n].index].prev_state.co); + add_v3_v3(vec, bbd->sim->psys->particles[ptn[n].index].prev_state.vel); + } + + mul_v3_fl(loc, 1.0f / ((float)neighbors - 1.0f)); + mul_v3_fl(vec, 1.0f / ((float)neighbors - 1.0f)); + + sub_v3_v3(loc, pa->prev_state.co); + sub_v3_v3(vec, pa->prev_state.vel); + + add_v3_v3(bbd->wanted_co, vec); + add_v3_v3(bbd->wanted_co, loc); + bbd->wanted_speed = len_v3(bbd->wanted_co); + + ret = 1; + } + return ret; } -static int rule_follow_leader(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa) +static int rule_follow_leader(BoidRule *rule, + BoidBrainData *bbd, + BoidValues *val, + ParticleData *pa) { - BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader*) rule; - float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; - float mul, len; - int n = (flbr->queue_size <= 1) ? bbd->sim->psys->totpart : flbr->queue_size; - int i, ret = 0, p = pa - bbd->sim->psys->particles; - - if (flbr->ob) { - float vec2[3], t; - - /* first check we're not blocking the leader */ - sub_v3_v3v3(vec, flbr->loc, flbr->oloc); - mul_v3_fl(vec, 1.0f/bbd->timestep); - - sub_v3_v3v3(loc, pa->prev_state.co, flbr->oloc); - - mul = dot_v3v3(vec, vec); - - /* leader is not moving */ - if (mul < 0.01f) { - len = len_v3(loc); - /* too close to leader */ - if (len < 2.0f * val->personal_space * pa->size) { - copy_v3_v3(bbd->wanted_co, loc); - bbd->wanted_speed = val->max_speed; - return 1; - } - } - else { - t = dot_v3v3(loc, vec)/mul; - - /* possible blocking of leader in near future */ - if (t > 0.0f && t < 3.0f) { - copy_v3_v3(vec2, vec); - mul_v3_fl(vec2, t); - - sub_v3_v3v3(vec2, loc, vec2); - - len = len_v3(vec2); - - if (len < 2.0f * val->personal_space * pa->size) { - copy_v3_v3(bbd->wanted_co, vec2); - bbd->wanted_speed = val->max_speed * (3.0f - t)/3.0f; - return 1; - } - } - } - - /* not blocking so try to follow leader */ - if (p && flbr->options & BRULE_LEADER_IN_LINE) { - copy_v3_v3(vec, bbd->sim->psys->particles[p-1].prev_state.vel); - copy_v3_v3(loc, bbd->sim->psys->particles[p-1].prev_state.co); - } - else { - copy_v3_v3(loc, flbr->oloc); - sub_v3_v3v3(vec, flbr->loc, flbr->oloc); - mul_v3_fl(vec, 1.0f/bbd->timestep); - } - - /* fac is seconds behind leader */ - madd_v3_v3fl(loc, vec, -flbr->distance); - - sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co); - bbd->wanted_speed = len_v3(bbd->wanted_co); - - ret = 1; - } - else if (p % n) { - float vec2[3], t, t_min = 3.0f; - - /* first check we're not blocking any leaders */ - for (i = 0; i< bbd->sim->psys->totpart; i+=n) { - copy_v3_v3(vec, bbd->sim->psys->particles[i].prev_state.vel); - - sub_v3_v3v3(loc, pa->prev_state.co, bbd->sim->psys->particles[i].prev_state.co); - - mul = dot_v3v3(vec, vec); - - /* leader is not moving */ - if (mul < 0.01f) { - len = len_v3(loc); - /* too close to leader */ - if (len < 2.0f * val->personal_space * pa->size) { - copy_v3_v3(bbd->wanted_co, loc); - bbd->wanted_speed = val->max_speed; - return 1; - } - } - else { - t = dot_v3v3(loc, vec)/mul; - - /* possible blocking of leader in near future */ - if (t > 0.0f && t < t_min) { - copy_v3_v3(vec2, vec); - mul_v3_fl(vec2, t); - - sub_v3_v3v3(vec2, loc, vec2); - - len = len_v3(vec2); - - if (len < 2.0f * val->personal_space * pa->size) { - t_min = t; - copy_v3_v3(bbd->wanted_co, loc); - bbd->wanted_speed = val->max_speed * (3.0f - t)/3.0f; - ret = 1; - } - } - } - } - - if (ret) return 1; - - /* not blocking so try to follow leader */ - if (flbr->options & BRULE_LEADER_IN_LINE) { - copy_v3_v3(vec, bbd->sim->psys->particles[p-1].prev_state.vel); - copy_v3_v3(loc, bbd->sim->psys->particles[p-1].prev_state.co); - } - else { - copy_v3_v3(vec, bbd->sim->psys->particles[p - p%n].prev_state.vel); - copy_v3_v3(loc, bbd->sim->psys->particles[p - p%n].prev_state.co); - } - - /* fac is seconds behind leader */ - madd_v3_v3fl(loc, vec, -flbr->distance); - - sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co); - bbd->wanted_speed = len_v3(bbd->wanted_co); - - ret = 1; - } - - return ret; + BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader *)rule; + float vec[3] = {0.0f, 0.0f, 0.0f}, loc[3] = {0.0f, 0.0f, 0.0f}; + float mul, len; + int n = (flbr->queue_size <= 1) ? bbd->sim->psys->totpart : flbr->queue_size; + int i, ret = 0, p = pa - bbd->sim->psys->particles; + + if (flbr->ob) { + float vec2[3], t; + + /* first check we're not blocking the leader */ + sub_v3_v3v3(vec, flbr->loc, flbr->oloc); + mul_v3_fl(vec, 1.0f / bbd->timestep); + + sub_v3_v3v3(loc, pa->prev_state.co, flbr->oloc); + + mul = dot_v3v3(vec, vec); + + /* leader is not moving */ + if (mul < 0.01f) { + len = len_v3(loc); + /* too close to leader */ + if (len < 2.0f * val->personal_space * pa->size) { + copy_v3_v3(bbd->wanted_co, loc); + bbd->wanted_speed = val->max_speed; + return 1; + } + } + else { + t = dot_v3v3(loc, vec) / mul; + + /* possible blocking of leader in near future */ + if (t > 0.0f && t < 3.0f) { + copy_v3_v3(vec2, vec); + mul_v3_fl(vec2, t); + + sub_v3_v3v3(vec2, loc, vec2); + + len = len_v3(vec2); + + if (len < 2.0f * val->personal_space * pa->size) { + copy_v3_v3(bbd->wanted_co, vec2); + bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f; + return 1; + } + } + } + + /* not blocking so try to follow leader */ + if (p && flbr->options & BRULE_LEADER_IN_LINE) { + copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel); + copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co); + } + else { + copy_v3_v3(loc, flbr->oloc); + sub_v3_v3v3(vec, flbr->loc, flbr->oloc); + mul_v3_fl(vec, 1.0f / bbd->timestep); + } + + /* fac is seconds behind leader */ + madd_v3_v3fl(loc, vec, -flbr->distance); + + sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co); + bbd->wanted_speed = len_v3(bbd->wanted_co); + + ret = 1; + } + else if (p % n) { + float vec2[3], t, t_min = 3.0f; + + /* first check we're not blocking any leaders */ + for (i = 0; i < bbd->sim->psys->totpart; i += n) { + copy_v3_v3(vec, bbd->sim->psys->particles[i].prev_state.vel); + + sub_v3_v3v3(loc, pa->prev_state.co, bbd->sim->psys->particles[i].prev_state.co); + + mul = dot_v3v3(vec, vec); + + /* leader is not moving */ + if (mul < 0.01f) { + len = len_v3(loc); + /* too close to leader */ + if (len < 2.0f * val->personal_space * pa->size) { + copy_v3_v3(bbd->wanted_co, loc); + bbd->wanted_speed = val->max_speed; + return 1; + } + } + else { + t = dot_v3v3(loc, vec) / mul; + + /* possible blocking of leader in near future */ + if (t > 0.0f && t < t_min) { + copy_v3_v3(vec2, vec); + mul_v3_fl(vec2, t); + + sub_v3_v3v3(vec2, loc, vec2); + + len = len_v3(vec2); + + if (len < 2.0f * val->personal_space * pa->size) { + t_min = t; + copy_v3_v3(bbd->wanted_co, loc); + bbd->wanted_speed = val->max_speed * (3.0f - t) / 3.0f; + ret = 1; + } + } + } + } + + if (ret) + return 1; + + /* not blocking so try to follow leader */ + if (flbr->options & BRULE_LEADER_IN_LINE) { + copy_v3_v3(vec, bbd->sim->psys->particles[p - 1].prev_state.vel); + copy_v3_v3(loc, bbd->sim->psys->particles[p - 1].prev_state.co); + } + else { + copy_v3_v3(vec, bbd->sim->psys->particles[p - p % n].prev_state.vel); + copy_v3_v3(loc, bbd->sim->psys->particles[p - p % n].prev_state.co); + } + + /* fac is seconds behind leader */ + madd_v3_v3fl(loc, vec, -flbr->distance); + + sub_v3_v3v3(bbd->wanted_co, loc, pa->prev_state.co); + bbd->wanted_speed = len_v3(bbd->wanted_co); + + ret = 1; + } + + return ret; } -static int rule_average_speed(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa) +static int rule_average_speed(BoidRule *rule, + BoidBrainData *bbd, + BoidValues *val, + ParticleData *pa) { - BoidParticle *bpa = pa->boid; - BoidRuleAverageSpeed *asbr = (BoidRuleAverageSpeed*)rule; - float vec[3] = {0.0f, 0.0f, 0.0f}; - - if (asbr->wander > 0.0f) { - /* abuse pa->r_ave for wandering */ - bpa->wander[0] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); - bpa->wander[1] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); - bpa->wander[2] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); - - normalize_v3(bpa->wander); - - copy_v3_v3(vec, bpa->wander); - - mul_qt_v3(pa->prev_state.rot, vec); - - copy_v3_v3(bbd->wanted_co, pa->prev_state.ave); - - mul_v3_fl(bbd->wanted_co, 1.1f); - - add_v3_v3(bbd->wanted_co, vec); - - /* leveling */ - if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) { - project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity); - mul_v3_fl(vec, asbr->level); - sub_v3_v3(bbd->wanted_co, vec); - } - } - else { - copy_v3_v3(bbd->wanted_co, pa->prev_state.ave); - - /* may happen at birth */ - if (dot_v2v2(bbd->wanted_co, bbd->wanted_co)==0.0f) { - bbd->wanted_co[0] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - bbd->wanted_co[1] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - bbd->wanted_co[2] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - } - - /* leveling */ - if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) { - project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity); - mul_v3_fl(vec, asbr->level); - sub_v3_v3(bbd->wanted_co, vec); - } - - } - bbd->wanted_speed = asbr->speed * val->max_speed; - - return 1; + BoidParticle *bpa = pa->boid; + BoidRuleAverageSpeed *asbr = (BoidRuleAverageSpeed *)rule; + float vec[3] = {0.0f, 0.0f, 0.0f}; + + if (asbr->wander > 0.0f) { + /* abuse pa->r_ave for wandering */ + bpa->wander[0] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); + bpa->wander[1] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); + bpa->wander[2] += asbr->wander * (-1.0f + 2.0f * BLI_rng_get_float(bbd->rng)); + + normalize_v3(bpa->wander); + + copy_v3_v3(vec, bpa->wander); + + mul_qt_v3(pa->prev_state.rot, vec); + + copy_v3_v3(bbd->wanted_co, pa->prev_state.ave); + + mul_v3_fl(bbd->wanted_co, 1.1f); + + add_v3_v3(bbd->wanted_co, vec); + + /* leveling */ + if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) { + project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity); + mul_v3_fl(vec, asbr->level); + sub_v3_v3(bbd->wanted_co, vec); + } + } + else { + copy_v3_v3(bbd->wanted_co, pa->prev_state.ave); + + /* may happen at birth */ + if (dot_v2v2(bbd->wanted_co, bbd->wanted_co) == 0.0f) { + bbd->wanted_co[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + bbd->wanted_co[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + bbd->wanted_co[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + } + + /* leveling */ + if (asbr->level > 0.0f && psys_uses_gravity(bbd->sim)) { + project_v3_v3v3(vec, bbd->wanted_co, bbd->sim->scene->physics_settings.gravity); + mul_v3_fl(vec, asbr->level); + sub_v3_v3(bbd->wanted_co, vec); + } + } + bbd->wanted_speed = asbr->speed * val->max_speed; + + return 1; } static int rule_fight(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa) { - BoidRuleFight *fbr = (BoidRuleFight*)rule; - KDTreeNearest_3d *ptn = NULL; - ParticleTarget *pt; - ParticleData *epars; - ParticleData *enemy_pa = NULL; - BoidParticle *bpa; - /* friends & enemies */ - float closest_enemy[3] = {0.0f, 0.0f, 0.0f}; - float closest_dist = fbr->distance + 1.0f; - float f_strength = 0.0f, e_strength = 0.0f; - float health = 0.0f; - int n, ret = 0; - - /* calculate own group strength */ - int neighbors = BLI_kdtree_3d_range_search( - bbd->sim->psys->tree, pa->prev_state.co, - &ptn, fbr->distance); - for (n=0; n<neighbors; n++) { - bpa = bbd->sim->psys->particles[ptn[n].index].boid; - health += bpa->data.health; - } - - f_strength += bbd->part->boids->strength * health; - - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - - /* add other friendlies and calculate enemy strength and find closest enemy */ - for (pt=bbd->sim->psys->targets.first; pt; pt=pt->next) { - ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); - if (epsys) { - epars = epsys->particles; - - neighbors = BLI_kdtree_3d_range_search( - epsys->tree, pa->prev_state.co, - &ptn, fbr->distance); - - health = 0.0f; - - for (n=0; n<neighbors; n++) { - bpa = epars[ptn[n].index].boid; - health += bpa->data.health; - - if (n==0 && pt->mode==PTARGET_MODE_ENEMY && ptn[n].dist < closest_dist) { - copy_v3_v3(closest_enemy, ptn[n].co); - closest_dist = ptn[n].dist; - enemy_pa = epars + ptn[n].index; - } - } - if (pt->mode==PTARGET_MODE_ENEMY) - e_strength += epsys->part->boids->strength * health; - else if (pt->mode==PTARGET_MODE_FRIEND) - f_strength += epsys->part->boids->strength * health; - - if (ptn) { MEM_freeN(ptn); ptn=NULL; } - } - } - /* decide action if enemy presence found */ - if (e_strength > 0.0f) { - sub_v3_v3v3(bbd->wanted_co, closest_enemy, pa->prev_state.co); - - /* attack if in range */ - if (closest_dist <= bbd->part->boids->range + pa->size + enemy_pa->size) { - float damage = BLI_rng_get_float(bbd->rng); - float enemy_dir[3]; - - normalize_v3_v3(enemy_dir, bbd->wanted_co); - - /* fight mode */ - bbd->wanted_speed = 0.0f; - - /* must face enemy to fight */ - if (dot_v3v3(pa->prev_state.ave, enemy_dir)>0.5f) { - bpa = enemy_pa->boid; - bpa->data.health -= bbd->part->boids->strength * bbd->timestep * ((1.0f-bbd->part->boids->accuracy)*damage + bbd->part->boids->accuracy); - } - } - else { - /* approach mode */ - bbd->wanted_speed = val->max_speed; - } - - /* check if boid doesn't want to fight */ - bpa = pa->boid; - if (bpa->data.health/bbd->part->boids->health * bbd->part->boids->aggression < e_strength / f_strength) { - /* decide to flee */ - if (closest_dist < fbr->flee_distance * fbr->distance) { - negate_v3(bbd->wanted_co); - bbd->wanted_speed = val->max_speed; - } - else { /* wait for better odds */ - bbd->wanted_speed = 0.0f; - } - } - - ret = 1; - } - - return ret; + BoidRuleFight *fbr = (BoidRuleFight *)rule; + KDTreeNearest_3d *ptn = NULL; + ParticleTarget *pt; + ParticleData *epars; + ParticleData *enemy_pa = NULL; + BoidParticle *bpa; + /* friends & enemies */ + float closest_enemy[3] = {0.0f, 0.0f, 0.0f}; + float closest_dist = fbr->distance + 1.0f; + float f_strength = 0.0f, e_strength = 0.0f; + float health = 0.0f; + int n, ret = 0; + + /* calculate own group strength */ + int neighbors = BLI_kdtree_3d_range_search( + bbd->sim->psys->tree, pa->prev_state.co, &ptn, fbr->distance); + for (n = 0; n < neighbors; n++) { + bpa = bbd->sim->psys->particles[ptn[n].index].boid; + health += bpa->data.health; + } + + f_strength += bbd->part->boids->strength * health; + + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + + /* add other friendlies and calculate enemy strength and find closest enemy */ + for (pt = bbd->sim->psys->targets.first; pt; pt = pt->next) { + ParticleSystem *epsys = psys_get_target_system(bbd->sim->ob, pt); + if (epsys) { + epars = epsys->particles; + + neighbors = BLI_kdtree_3d_range_search(epsys->tree, pa->prev_state.co, &ptn, fbr->distance); + + health = 0.0f; + + for (n = 0; n < neighbors; n++) { + bpa = epars[ptn[n].index].boid; + health += bpa->data.health; + + if (n == 0 && pt->mode == PTARGET_MODE_ENEMY && ptn[n].dist < closest_dist) { + copy_v3_v3(closest_enemy, ptn[n].co); + closest_dist = ptn[n].dist; + enemy_pa = epars + ptn[n].index; + } + } + if (pt->mode == PTARGET_MODE_ENEMY) + e_strength += epsys->part->boids->strength * health; + else if (pt->mode == PTARGET_MODE_FRIEND) + f_strength += epsys->part->boids->strength * health; + + if (ptn) { + MEM_freeN(ptn); + ptn = NULL; + } + } + } + /* decide action if enemy presence found */ + if (e_strength > 0.0f) { + sub_v3_v3v3(bbd->wanted_co, closest_enemy, pa->prev_state.co); + + /* attack if in range */ + if (closest_dist <= bbd->part->boids->range + pa->size + enemy_pa->size) { + float damage = BLI_rng_get_float(bbd->rng); + float enemy_dir[3]; + + normalize_v3_v3(enemy_dir, bbd->wanted_co); + + /* fight mode */ + bbd->wanted_speed = 0.0f; + + /* must face enemy to fight */ + if (dot_v3v3(pa->prev_state.ave, enemy_dir) > 0.5f) { + bpa = enemy_pa->boid; + bpa->data.health -= bbd->part->boids->strength * bbd->timestep * + ((1.0f - bbd->part->boids->accuracy) * damage + + bbd->part->boids->accuracy); + } + } + else { + /* approach mode */ + bbd->wanted_speed = val->max_speed; + } + + /* check if boid doesn't want to fight */ + bpa = pa->boid; + if (bpa->data.health / bbd->part->boids->health * bbd->part->boids->aggression < + e_strength / f_strength) { + /* decide to flee */ + if (closest_dist < fbr->flee_distance * fbr->distance) { + negate_v3(bbd->wanted_co); + bbd->wanted_speed = val->max_speed; + } + else { /* wait for better odds */ + bbd->wanted_speed = 0.0f; + } + } + + ret = 1; + } + + return ret; } -typedef int (*boid_rule_cb)(BoidRule *rule, BoidBrainData *data, BoidValues *val, ParticleData *pa); +typedef int (*boid_rule_cb)(BoidRule *rule, + BoidBrainData *data, + BoidValues *val, + ParticleData *pa); static boid_rule_cb boid_rules[] = { - rule_none, - rule_goal_avoid, - rule_goal_avoid, - rule_avoid_collision, - rule_separate, - rule_flock, - rule_follow_leader, - rule_average_speed, - rule_fight, - //rule_help, - //rule_protect, - //rule_hide, - //rule_follow_path, - //rule_follow_wall, + rule_none, + rule_goal_avoid, + rule_goal_avoid, + rule_avoid_collision, + rule_separate, + rule_flock, + rule_follow_leader, + rule_average_speed, + rule_fight, + //rule_help, + //rule_protect, + //rule_hide, + //rule_follow_path, + //rule_follow_wall, }; static void set_boid_values(BoidValues *val, BoidSettings *boids, ParticleData *pa) { - BoidParticle *bpa = pa->boid; - - if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) { - val->max_speed = boids->land_max_speed * bpa->data.health/boids->health; - val->max_acc = boids->land_max_acc * val->max_speed; - val->max_ave = boids->land_max_ave * (float)M_PI * bpa->data.health/boids->health; - val->min_speed = 0.0f; /* no minimum speed on land */ - val->personal_space = boids->land_personal_space; - val->jump_speed = boids->land_jump_speed * bpa->data.health/boids->health; - } - else { - val->max_speed = boids->air_max_speed * bpa->data.health/boids->health; - val->max_acc = boids->air_max_acc * val->max_speed; - val->max_ave = boids->air_max_ave * (float)M_PI * bpa->data.health/boids->health; - val->min_speed = boids->air_min_speed * boids->air_max_speed; - val->personal_space = boids->air_personal_space; - val->jump_speed = 0.0f; /* no jumping in air */ - } + BoidParticle *bpa = pa->boid; + + if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) { + val->max_speed = boids->land_max_speed * bpa->data.health / boids->health; + val->max_acc = boids->land_max_acc * val->max_speed; + val->max_ave = boids->land_max_ave * (float)M_PI * bpa->data.health / boids->health; + val->min_speed = 0.0f; /* no minimum speed on land */ + val->personal_space = boids->land_personal_space; + val->jump_speed = boids->land_jump_speed * bpa->data.health / boids->health; + } + else { + val->max_speed = boids->air_max_speed * bpa->data.health / boids->health; + val->max_acc = boids->air_max_acc * val->max_speed; + val->max_ave = boids->air_max_ave * (float)M_PI * bpa->data.health / boids->health; + val->min_speed = boids->air_min_speed * boids->air_max_speed; + val->personal_space = boids->air_personal_space; + val->jump_speed = 0.0f; /* no jumping in air */ + } } -static Object *boid_find_ground(BoidBrainData *bbd, ParticleData *pa, float ground_co[3], float ground_nor[3]) +static Object *boid_find_ground(BoidBrainData *bbd, + ParticleData *pa, + float ground_co[3], + float ground_nor[3]) { - const int raycast_flag = BVH_RAYCAST_DEFAULT & ~(BVH_RAYCAST_WATERTIGHT); - BoidParticle *bpa = pa->boid; - - if (bpa->data.mode == eBoidMode_Climbing) { - SurfaceModifierData *surmd = NULL; - float x[3], v[3]; - - surmd = (SurfaceModifierData *)modifiers_findByType(bpa->ground, eModifierType_Surface ); - - /* take surface velocity into account */ - closest_point_on_surface(surmd, pa->state.co, x, NULL, v); - add_v3_v3(x, v); - - /* get actual position on surface */ - closest_point_on_surface(surmd, x, ground_co, ground_nor, NULL); - - return bpa->ground; - } - else { - float zvec[3] = {0.0f, 0.0f, 2000.0f}; - ParticleCollision col; - ColliderCache *coll; - BVHTreeRayHit hit; - float radius = 0.0f, t, ray_dir[3]; - - if (!bbd->sim->colliders) - return NULL; - - memset(&col, 0, sizeof(ParticleCollision)); - - /* first try to find below boid */ - copy_v3_v3(col.co1, pa->state.co); - sub_v3_v3v3(col.co2, pa->state.co, zvec); - sub_v3_v3v3(ray_dir, col.co2, col.co1); - col.f = 0.0f; - hit.index = -1; - hit.dist = col.original_ray_length = normalize_v3(ray_dir); - col.pce.inside = 0; - - for (coll = bbd->sim->colliders->first; coll; coll = coll->next) { - col.current = coll->ob; - col.md = coll->collmd; - col.fac1 = col.fac2 = 0.f; - - if (col.md && col.md->bvhtree) { - BLI_bvhtree_ray_cast_ex( - col.md->bvhtree, col.co1, ray_dir, radius, &hit, - BKE_psys_collision_neartest_cb, &col, raycast_flag); - } - } - /* then use that object */ - if (hit.index>=0) { - t = hit.dist/col.original_ray_length; - interp_v3_v3v3(ground_co, col.co1, col.co2, t); - normalize_v3_v3(ground_nor, col.pce.nor); - return col.hit; - } - - /* couldn't find below, so find upmost deflector object */ - add_v3_v3v3(col.co1, pa->state.co, zvec); - sub_v3_v3v3(col.co2, pa->state.co, zvec); - sub_v3_v3(col.co2, zvec); - sub_v3_v3v3(ray_dir, col.co2, col.co1); - col.f = 0.0f; - hit.index = -1; - hit.dist = col.original_ray_length = normalize_v3(ray_dir); - - for (coll = bbd->sim->colliders->first; coll; coll = coll->next) { - col.current = coll->ob; - col.md = coll->collmd; - - if (col.md && col.md->bvhtree) { - BLI_bvhtree_ray_cast_ex( - col.md->bvhtree, col.co1, ray_dir, radius, &hit, - BKE_psys_collision_neartest_cb, &col, raycast_flag); - } - } - /* then use that object */ - if (hit.index>=0) { - t = hit.dist/col.original_ray_length; - interp_v3_v3v3(ground_co, col.co1, col.co2, t); - normalize_v3_v3(ground_nor, col.pce.nor); - return col.hit; - } - - /* default to z=0 */ - copy_v3_v3(ground_co, pa->state.co); - ground_co[2] = 0; - ground_nor[0] = ground_nor[1] = 0.0f; - ground_nor[2] = 1.0f; - return NULL; - } + const int raycast_flag = BVH_RAYCAST_DEFAULT & ~(BVH_RAYCAST_WATERTIGHT); + BoidParticle *bpa = pa->boid; + + if (bpa->data.mode == eBoidMode_Climbing) { + SurfaceModifierData *surmd = NULL; + float x[3], v[3]; + + surmd = (SurfaceModifierData *)modifiers_findByType(bpa->ground, eModifierType_Surface); + + /* take surface velocity into account */ + closest_point_on_surface(surmd, pa->state.co, x, NULL, v); + add_v3_v3(x, v); + + /* get actual position on surface */ + closest_point_on_surface(surmd, x, ground_co, ground_nor, NULL); + + return bpa->ground; + } + else { + float zvec[3] = {0.0f, 0.0f, 2000.0f}; + ParticleCollision col; + ColliderCache *coll; + BVHTreeRayHit hit; + float radius = 0.0f, t, ray_dir[3]; + + if (!bbd->sim->colliders) + return NULL; + + memset(&col, 0, sizeof(ParticleCollision)); + + /* first try to find below boid */ + copy_v3_v3(col.co1, pa->state.co); + sub_v3_v3v3(col.co2, pa->state.co, zvec); + sub_v3_v3v3(ray_dir, col.co2, col.co1); + col.f = 0.0f; + hit.index = -1; + hit.dist = col.original_ray_length = normalize_v3(ray_dir); + col.pce.inside = 0; + + for (coll = bbd->sim->colliders->first; coll; coll = coll->next) { + col.current = coll->ob; + col.md = coll->collmd; + col.fac1 = col.fac2 = 0.f; + + if (col.md && col.md->bvhtree) { + BLI_bvhtree_ray_cast_ex(col.md->bvhtree, + col.co1, + ray_dir, + radius, + &hit, + BKE_psys_collision_neartest_cb, + &col, + raycast_flag); + } + } + /* then use that object */ + if (hit.index >= 0) { + t = hit.dist / col.original_ray_length; + interp_v3_v3v3(ground_co, col.co1, col.co2, t); + normalize_v3_v3(ground_nor, col.pce.nor); + return col.hit; + } + + /* couldn't find below, so find upmost deflector object */ + add_v3_v3v3(col.co1, pa->state.co, zvec); + sub_v3_v3v3(col.co2, pa->state.co, zvec); + sub_v3_v3(col.co2, zvec); + sub_v3_v3v3(ray_dir, col.co2, col.co1); + col.f = 0.0f; + hit.index = -1; + hit.dist = col.original_ray_length = normalize_v3(ray_dir); + + for (coll = bbd->sim->colliders->first; coll; coll = coll->next) { + col.current = coll->ob; + col.md = coll->collmd; + + if (col.md && col.md->bvhtree) { + BLI_bvhtree_ray_cast_ex(col.md->bvhtree, + col.co1, + ray_dir, + radius, + &hit, + BKE_psys_collision_neartest_cb, + &col, + raycast_flag); + } + } + /* then use that object */ + if (hit.index >= 0) { + t = hit.dist / col.original_ray_length; + interp_v3_v3v3(ground_co, col.co1, col.co2, t); + normalize_v3_v3(ground_nor, col.pce.nor); + return col.hit; + } + + /* default to z=0 */ + copy_v3_v3(ground_co, pa->state.co); + ground_co[2] = 0; + ground_nor[0] = ground_nor[1] = 0.0f; + ground_nor[2] = 1.0f; + return NULL; + } } static int boid_rule_applies(ParticleData *pa, BoidSettings *UNUSED(boids), BoidRule *rule) { - BoidParticle *bpa = pa->boid; + BoidParticle *bpa = pa->boid; - if (rule==NULL) - return 0; + if (rule == NULL) + return 0; - if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) && rule->flag & BOIDRULE_ON_LAND) - return 1; + if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) && rule->flag & BOIDRULE_ON_LAND) + return 1; - if (bpa->data.mode==eBoidMode_InAir && rule->flag & BOIDRULE_IN_AIR) - return 1; + if (bpa->data.mode == eBoidMode_InAir && rule->flag & BOIDRULE_IN_AIR) + return 1; - return 0; + return 0; } void boids_precalc_rules(ParticleSettings *part, float cfra) { - BoidState *state = part->boids->states.first; - BoidRule *rule; - for (; state; state=state->next) { - for (rule = state->rules.first; rule; rule=rule->next) { - if (rule->type==eBoidRuleType_FollowLeader) { - BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader*) rule; - - if (flbr->ob && flbr->cfra != cfra) { - /* save object locations for velocity calculations */ - copy_v3_v3(flbr->oloc, flbr->loc); - copy_v3_v3(flbr->loc, flbr->ob->obmat[3]); - flbr->cfra = cfra; - } - } - } - } + BoidState *state = part->boids->states.first; + BoidRule *rule; + for (; state; state = state->next) { + for (rule = state->rules.first; rule; rule = rule->next) { + if (rule->type == eBoidRuleType_FollowLeader) { + BoidRuleFollowLeader *flbr = (BoidRuleFollowLeader *)rule; + + if (flbr->ob && flbr->cfra != cfra) { + /* save object locations for velocity calculations */ + copy_v3_v3(flbr->oloc, flbr->loc); + copy_v3_v3(flbr->loc, flbr->ob->obmat[3]); + flbr->cfra = cfra; + } + } + } + } } -static void boid_climb(BoidSettings *boids, ParticleData *pa, float *surface_co, float *surface_nor) +static void boid_climb(BoidSettings *boids, + ParticleData *pa, + float *surface_co, + float *surface_nor) { - BoidParticle *bpa = pa->boid; - float nor[3], vel[3]; - copy_v3_v3(nor, surface_nor); + BoidParticle *bpa = pa->boid; + float nor[3], vel[3]; + copy_v3_v3(nor, surface_nor); - /* gather apparent gravity */ - madd_v3_v3fl(bpa->gravity, surface_nor, -1.0f); - normalize_v3(bpa->gravity); + /* gather apparent gravity */ + madd_v3_v3fl(bpa->gravity, surface_nor, -1.0f); + normalize_v3(bpa->gravity); - /* raise boid it's size from surface */ - mul_v3_fl(nor, pa->size * boids->height); - add_v3_v3v3(pa->state.co, surface_co, nor); + /* raise boid it's size from surface */ + mul_v3_fl(nor, pa->size * boids->height); + add_v3_v3v3(pa->state.co, surface_co, nor); - /* remove normal component from velocity */ - project_v3_v3v3(vel, pa->state.vel, surface_nor); - sub_v3_v3v3(pa->state.vel, pa->state.vel, vel); + /* remove normal component from velocity */ + project_v3_v3v3(vel, pa->state.vel, surface_nor); + sub_v3_v3v3(pa->state.vel, pa->state.vel, vel); } static float boid_goal_signed_dist(float *boid_co, float *goal_co, float *goal_nor) { - float vec[3]; + float vec[3]; - sub_v3_v3v3(vec, boid_co, goal_co); + sub_v3_v3v3(vec, boid_co, goal_co); - return dot_v3v3(vec, goal_nor); + return dot_v3v3(vec, goal_nor); } /* wanted_co is relative to boid location */ -static int apply_boid_rule(BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness) +static int apply_boid_rule( + BoidBrainData *bbd, BoidRule *rule, BoidValues *val, ParticleData *pa, float fuzziness) { - if (rule==NULL) - return 0; + if (rule == NULL) + return 0; - if (boid_rule_applies(pa, bbd->part->boids, rule)==0) - return 0; + if (boid_rule_applies(pa, bbd->part->boids, rule) == 0) + return 0; - if (boid_rules[rule->type](rule, bbd, val, pa)==0) - return 0; + if (boid_rules[rule->type](rule, bbd, val, pa) == 0) + return 0; - if (fuzziness < 0.0f || compare_len_v3v3(bbd->wanted_co, pa->prev_state.vel, fuzziness * len_v3(pa->prev_state.vel))==0) - return 1; - else - return 0; + if (fuzziness < 0.0f || compare_len_v3v3(bbd->wanted_co, + pa->prev_state.vel, + fuzziness * len_v3(pa->prev_state.vel)) == 0) + return 1; + else + return 0; } static BoidState *get_boid_state(BoidSettings *boids, ParticleData *pa) { - BoidState *state = boids->states.first; - BoidParticle *bpa = pa->boid; + BoidState *state = boids->states.first; + BoidParticle *bpa = pa->boid; - for (; state; state=state->next) { - if (state->id==bpa->data.state_id) - return state; - } + for (; state; state = state->next) { + if (state->id == bpa->data.state_id) + return state; + } - /* for some reason particle isn't at a valid state */ - state = boids->states.first; - if (state) - bpa->data.state_id = state->id; + /* for some reason particle isn't at a valid state */ + state = boids->states.first; + if (state) + bpa->data.state_id = state->id; - return state; + return state; } //static int boid_condition_is_true(BoidCondition *cond) //{ -// /* TODO */ -// return 0; +// /* TODO */ +// return 0; //} /* determines the velocity the boid wants to have */ void boid_brain(BoidBrainData *bbd, int p, ParticleData *pa) { - BoidRule *rule; - BoidSettings *boids = bbd->part->boids; - BoidValues val; - BoidState *state = get_boid_state(boids, pa); - BoidParticle *bpa = pa->boid; - ParticleSystem *psys = bbd->sim->psys; - int rand; - //BoidCondition *cond; - - if (bpa->data.health <= 0.0f) { - pa->alive = PARS_DYING; - pa->dietime = bbd->cfra; - return; - } - - //planned for near future - //cond = state->conditions.first; - //for (; cond; cond=cond->next) { - // if (boid_condition_is_true(cond)) { - // pa->boid->state_id = cond->state_id; - // state = get_boid_state(boids, pa); - // break; /* only first true condition is used */ - // } - //} - - zero_v3(bbd->wanted_co); - bbd->wanted_speed = 0.0f; - - /* create random seed for every particle & frame */ - rand = (int)(psys_frand(psys, psys->seed + p) * 1000); - rand = (int)(psys_frand(psys, (int)bbd->cfra + rand) * 1000); - - set_boid_values(&val, bbd->part->boids, pa); - - /* go through rules */ - switch (state->ruleset_type) { - case eBoidRulesetType_Fuzzy: - { - for (rule = state->rules.first; rule; rule = rule->next) { - if (apply_boid_rule(bbd, rule, &val, pa, state->rule_fuzziness)) - break; /* only first nonzero rule that comes through fuzzy rule is applied */ - } - break; - } - case eBoidRulesetType_Random: - { - /* use random rule for each particle (always same for same particle though) */ - const int n = BLI_listbase_count(&state->rules); - if (n) { - rule = BLI_findlink(&state->rules, rand % n); - apply_boid_rule(bbd, rule, &val, pa, -1.0); - } - break; - } - case eBoidRulesetType_Average: - { - float wanted_co[3] = {0.0f, 0.0f, 0.0f}, wanted_speed = 0.0f; - int n = 0; - for (rule = state->rules.first; rule; rule=rule->next) { - if (apply_boid_rule(bbd, rule, &val, pa, -1.0f)) { - add_v3_v3(wanted_co, bbd->wanted_co); - wanted_speed += bbd->wanted_speed; - n++; - zero_v3(bbd->wanted_co); - bbd->wanted_speed = 0.0f; - } - } - - if (n > 1) { - mul_v3_fl(wanted_co, 1.0f/(float)n); - wanted_speed /= (float)n; - } - - copy_v3_v3(bbd->wanted_co, wanted_co); - bbd->wanted_speed = wanted_speed; - break; - } - - } - - /* decide on jumping & liftoff */ - if (bpa->data.mode == eBoidMode_OnLand) { - /* fuzziness makes boids capable of misjudgement */ - float mul = 1.0f + state->rule_fuzziness; - - if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) { - float cvel[3], dir[3]; - - copy_v3_v3(dir, pa->prev_state.ave); - normalize_v2(dir); - - copy_v3_v3(cvel, bbd->wanted_co); - normalize_v2(cvel); - - if (dot_v2v2(cvel, dir) > 0.95f / mul) - bpa->data.mode = eBoidMode_Liftoff; - } - else if (val.jump_speed > 0.0f) { - float jump_v[3]; - int jump = 0; - - /* jump to get to a location */ - if (bbd->wanted_co[2] > 0.0f) { - float cvel[3], dir[3]; - float z_v, ground_v, cur_v; - float len; - - copy_v3_v3(dir, pa->prev_state.ave); - normalize_v2(dir); - - copy_v3_v3(cvel, bbd->wanted_co); - normalize_v2(cvel); - - len = len_v2(pa->prev_state.vel); - - /* first of all, are we going in a suitable direction? */ - /* or at a suitably slow speed */ - if (dot_v2v2(cvel, dir) > 0.95f / mul || len <= state->rule_fuzziness) { - /* try to reach goal at highest point of the parabolic path */ - cur_v = len_v2(pa->prev_state.vel); - z_v = sasqrt(-2.0f * bbd->sim->scene->physics_settings.gravity[2] * bbd->wanted_co[2]); - ground_v = len_v2(bbd->wanted_co)*sasqrt(-0.5f * bbd->sim->scene->physics_settings.gravity[2] / bbd->wanted_co[2]); - - len = sasqrt((ground_v-cur_v)*(ground_v-cur_v) + z_v*z_v); - - if (len < val.jump_speed * mul || bbd->part->boids->options & BOID_ALLOW_FLIGHT) { - jump = 1; - - len = MIN2(len, val.jump_speed); - - copy_v3_v3(jump_v, dir); - jump_v[2] = z_v; - mul_v3_fl(jump_v, ground_v); - - normalize_v3(jump_v); - mul_v3_fl(jump_v, len); - add_v2_v2v2(jump_v, jump_v, pa->prev_state.vel); - } - } - } - - /* jump to go faster */ - if (jump == 0 && val.jump_speed > val.max_speed && bbd->wanted_speed > val.max_speed) { - /* pass */ - } - - if (jump) { - copy_v3_v3(pa->prev_state.vel, jump_v); - bpa->data.mode = eBoidMode_Falling; - } - } - } + BoidRule *rule; + BoidSettings *boids = bbd->part->boids; + BoidValues val; + BoidState *state = get_boid_state(boids, pa); + BoidParticle *bpa = pa->boid; + ParticleSystem *psys = bbd->sim->psys; + int rand; + //BoidCondition *cond; + + if (bpa->data.health <= 0.0f) { + pa->alive = PARS_DYING; + pa->dietime = bbd->cfra; + return; + } + + //planned for near future + //cond = state->conditions.first; + //for (; cond; cond=cond->next) { + // if (boid_condition_is_true(cond)) { + // pa->boid->state_id = cond->state_id; + // state = get_boid_state(boids, pa); + // break; /* only first true condition is used */ + // } + //} + + zero_v3(bbd->wanted_co); + bbd->wanted_speed = 0.0f; + + /* create random seed for every particle & frame */ + rand = (int)(psys_frand(psys, psys->seed + p) * 1000); + rand = (int)(psys_frand(psys, (int)bbd->cfra + rand) * 1000); + + set_boid_values(&val, bbd->part->boids, pa); + + /* go through rules */ + switch (state->ruleset_type) { + case eBoidRulesetType_Fuzzy: { + for (rule = state->rules.first; rule; rule = rule->next) { + if (apply_boid_rule(bbd, rule, &val, pa, state->rule_fuzziness)) + break; /* only first nonzero rule that comes through fuzzy rule is applied */ + } + break; + } + case eBoidRulesetType_Random: { + /* use random rule for each particle (always same for same particle though) */ + const int n = BLI_listbase_count(&state->rules); + if (n) { + rule = BLI_findlink(&state->rules, rand % n); + apply_boid_rule(bbd, rule, &val, pa, -1.0); + } + break; + } + case eBoidRulesetType_Average: { + float wanted_co[3] = {0.0f, 0.0f, 0.0f}, wanted_speed = 0.0f; + int n = 0; + for (rule = state->rules.first; rule; rule = rule->next) { + if (apply_boid_rule(bbd, rule, &val, pa, -1.0f)) { + add_v3_v3(wanted_co, bbd->wanted_co); + wanted_speed += bbd->wanted_speed; + n++; + zero_v3(bbd->wanted_co); + bbd->wanted_speed = 0.0f; + } + } + + if (n > 1) { + mul_v3_fl(wanted_co, 1.0f / (float)n); + wanted_speed /= (float)n; + } + + copy_v3_v3(bbd->wanted_co, wanted_co); + bbd->wanted_speed = wanted_speed; + break; + } + } + + /* decide on jumping & liftoff */ + if (bpa->data.mode == eBoidMode_OnLand) { + /* fuzziness makes boids capable of misjudgement */ + float mul = 1.0f + state->rule_fuzziness; + + if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) { + float cvel[3], dir[3]; + + copy_v3_v3(dir, pa->prev_state.ave); + normalize_v2(dir); + + copy_v3_v3(cvel, bbd->wanted_co); + normalize_v2(cvel); + + if (dot_v2v2(cvel, dir) > 0.95f / mul) + bpa->data.mode = eBoidMode_Liftoff; + } + else if (val.jump_speed > 0.0f) { + float jump_v[3]; + int jump = 0; + + /* jump to get to a location */ + if (bbd->wanted_co[2] > 0.0f) { + float cvel[3], dir[3]; + float z_v, ground_v, cur_v; + float len; + + copy_v3_v3(dir, pa->prev_state.ave); + normalize_v2(dir); + + copy_v3_v3(cvel, bbd->wanted_co); + normalize_v2(cvel); + + len = len_v2(pa->prev_state.vel); + + /* first of all, are we going in a suitable direction? */ + /* or at a suitably slow speed */ + if (dot_v2v2(cvel, dir) > 0.95f / mul || len <= state->rule_fuzziness) { + /* try to reach goal at highest point of the parabolic path */ + cur_v = len_v2(pa->prev_state.vel); + z_v = sasqrt(-2.0f * bbd->sim->scene->physics_settings.gravity[2] * bbd->wanted_co[2]); + ground_v = len_v2(bbd->wanted_co) * + sasqrt(-0.5f * bbd->sim->scene->physics_settings.gravity[2] / + bbd->wanted_co[2]); + + len = sasqrt((ground_v - cur_v) * (ground_v - cur_v) + z_v * z_v); + + if (len < val.jump_speed * mul || bbd->part->boids->options & BOID_ALLOW_FLIGHT) { + jump = 1; + + len = MIN2(len, val.jump_speed); + + copy_v3_v3(jump_v, dir); + jump_v[2] = z_v; + mul_v3_fl(jump_v, ground_v); + + normalize_v3(jump_v); + mul_v3_fl(jump_v, len); + add_v2_v2v2(jump_v, jump_v, pa->prev_state.vel); + } + } + } + + /* jump to go faster */ + if (jump == 0 && val.jump_speed > val.max_speed && bbd->wanted_speed > val.max_speed) { + /* pass */ + } + + if (jump) { + copy_v3_v3(pa->prev_state.vel, jump_v); + bpa->data.mode = eBoidMode_Falling; + } + } + } } /* tries to realize the wanted velocity taking all constraints into account */ void boid_body(BoidBrainData *bbd, ParticleData *pa) { - BoidSettings *boids = bbd->part->boids; - BoidParticle *bpa = pa->boid; - BoidValues val; - EffectedPoint epoint; - float acc[3] = {0.0f, 0.0f, 0.0f}, tan_acc[3], nor_acc[3]; - float dvec[3], bvec[3]; - float new_dir[3], new_speed; - float old_dir[3], old_speed; - float wanted_dir[3]; - float q[4], mat[3][3]; /* rotation */ - float ground_co[3] = {0.0f, 0.0f, 0.0f}, ground_nor[3] = {0.0f, 0.0f, 1.0f}; - float force[3] = {0.0f, 0.0f, 0.0f}; - float pa_mass=bbd->part->mass, dtime=bbd->dfra*bbd->timestep; - - set_boid_values(&val, boids, pa); - - /* make sure there's something in new velocity, location & rotation */ - copy_particle_key(&pa->state, &pa->prev_state, 0); - - if (bbd->part->flag & PART_SIZEMASS) - pa_mass*=pa->size; - - /* if boids can't fly they fall to the ground */ - if ((boids->options & BOID_ALLOW_FLIGHT)==0 && ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)==0 && psys_uses_gravity(bbd->sim)) - bpa->data.mode = eBoidMode_Falling; - - if (bpa->data.mode == eBoidMode_Falling) { - /* Falling boids are only effected by gravity. */ - acc[2] = bbd->sim->scene->physics_settings.gravity[2]; - } - else { - /* figure out acceleration */ - float landing_level = 2.0f; - float level = landing_level + 1.0f; - float new_vel[3]; - - if (bpa->data.mode == eBoidMode_Liftoff) { - bpa->data.mode = eBoidMode_InAir; - bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); - } - else if (bpa->data.mode == eBoidMode_InAir && boids->options & BOID_ALLOW_LAND) { - /* auto-leveling & landing if close to ground */ - - bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); - - /* level = how many particle sizes above ground */ - level = (pa->prev_state.co[2] - ground_co[2])/(2.0f * pa->size) - 0.5f; - - landing_level = - boids->landing_smoothness * pa->prev_state.vel[2] * pa_mass; - - if (pa->prev_state.vel[2] < 0.0f) { - if (level < 1.0f) { - bbd->wanted_co[0] = bbd->wanted_co[1] = bbd->wanted_co[2] = 0.0f; - bbd->wanted_speed = 0.0f; - bpa->data.mode = eBoidMode_Falling; - } - else if (level < landing_level) { - bbd->wanted_speed *= (level - 1.0f)/landing_level; - bbd->wanted_co[2] *= (level - 1.0f)/landing_level; - } - } - } - - copy_v3_v3(old_dir, pa->prev_state.ave); - new_speed = normalize_v3_v3(wanted_dir, bbd->wanted_co); - - /* first check if we have valid direction we want to go towards */ - if (new_speed == 0.0f) { - copy_v3_v3(new_dir, old_dir); - } - else { - float old_dir2[2], wanted_dir2[2], nor[3], angle; - copy_v2_v2(old_dir2, old_dir); - normalize_v2(old_dir2); - copy_v2_v2(wanted_dir2, wanted_dir); - normalize_v2(wanted_dir2); - - /* choose random direction to turn if wanted velocity */ - /* is directly behind regardless of z-coordinate */ - if (dot_v2v2(old_dir2, wanted_dir2) < -0.99f) { - wanted_dir[0] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - wanted_dir[1] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - wanted_dir[2] = 2.0f*(0.5f - BLI_rng_get_float(bbd->rng)); - normalize_v3(wanted_dir); - } - - /* constrain direction with maximum angular velocity */ - angle = saacos(dot_v3v3(old_dir, wanted_dir)); - angle = min_ff(angle, val.max_ave); - - cross_v3_v3v3(nor, old_dir, wanted_dir); - axis_angle_to_quat(q, nor, angle); - copy_v3_v3(new_dir, old_dir); - mul_qt_v3(q, new_dir); - normalize_v3(new_dir); - - /* save direction in case resulting velocity too small */ - axis_angle_to_quat(q, nor, angle*dtime); - copy_v3_v3(pa->state.ave, old_dir); - mul_qt_v3(q, pa->state.ave); - normalize_v3(pa->state.ave); - } - - /* constrain speed with maximum acceleration */ - old_speed = len_v3(pa->prev_state.vel); - - if (bbd->wanted_speed < old_speed) - new_speed = MAX2(bbd->wanted_speed, old_speed - val.max_acc); - else - new_speed = MIN2(bbd->wanted_speed, old_speed + val.max_acc); - - /* combine direction and speed */ - copy_v3_v3(new_vel, new_dir); - mul_v3_fl(new_vel, new_speed); - - /* maintain minimum flying velocity if not landing */ - if (level >= landing_level) { - float len2 = dot_v2v2(new_vel, new_vel); - float root; - - len2 = MAX2(len2, val.min_speed*val.min_speed); - root = sasqrt(new_speed*new_speed - len2); - - new_vel[2] = new_vel[2] < 0.0f ? -root : root; - - normalize_v2(new_vel); - mul_v2_fl(new_vel, sasqrt(len2)); - } - - /* finally constrain speed to max speed */ - new_speed = normalize_v3(new_vel); - mul_v3_fl(new_vel, MIN2(new_speed, val.max_speed)); - - /* get acceleration from difference of velocities */ - sub_v3_v3v3(acc, new_vel, pa->prev_state.vel); - - /* break acceleration to components */ - project_v3_v3v3(tan_acc, acc, pa->prev_state.ave); - sub_v3_v3v3(nor_acc, acc, tan_acc); - } - - /* account for effectors */ - pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint); - BKE_effectors_apply(bbd->sim->psys->effectors, bbd->sim->colliders, bbd->part->effector_weights, &epoint, force, NULL); - - if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) { - float length = normalize_v3(force); - - length = MAX2(0.0f, length - boids->land_stick_force); - - mul_v3_fl(force, length); - } - - add_v3_v3(acc, force); - - /* store smoothed acceleration for nice banking etc. */ - madd_v3_v3fl(bpa->data.acc, acc, dtime); - mul_v3_fl(bpa->data.acc, 1.0f / (1.0f + dtime)); - - /* integrate new location & velocity */ - - /* by regarding the acceleration as a force at this stage we - * can get better control although it's a bit unphysical */ - mul_v3_fl(acc, 1.0f/pa_mass); - - copy_v3_v3(dvec, acc); - mul_v3_fl(dvec, dtime*dtime*0.5f); - - copy_v3_v3(bvec, pa->prev_state.vel); - mul_v3_fl(bvec, dtime); - add_v3_v3(dvec, bvec); - add_v3_v3(pa->state.co, dvec); - - madd_v3_v3fl(pa->state.vel, acc, dtime); - - //if (bpa->data.mode != eBoidMode_InAir) - bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); - - /* change modes, constrain movement & keep track of down vector */ - switch (bpa->data.mode) { - case eBoidMode_InAir: - { - float grav[3]; - - grav[0] = 0.0f; - grav[1] = 0.0f; - grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f; - - /* don't take forward acceleration into account (better banking) */ - if (dot_v3v3(bpa->data.acc, pa->state.vel) > 0.0f) { - project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel); - sub_v3_v3v3(dvec, bpa->data.acc, dvec); - } - else { - copy_v3_v3(dvec, bpa->data.acc); - } - - /* gather apparent gravity */ - madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking); - normalize_v3(bpa->gravity); - - /* stick boid on goal when close enough */ - if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= pa->size * boids->height) { - bpa->data.mode = eBoidMode_Climbing; - bpa->ground = bbd->goal_ob; - boid_find_ground(bbd, pa, ground_co, ground_nor); - boid_climb(boids, pa, ground_co, ground_nor); - } - else if (pa->state.co[2] <= ground_co[2] + pa->size * boids->height) { - /* land boid when below ground */ - if (boids->options & BOID_ALLOW_LAND) { - pa->state.co[2] = ground_co[2] + pa->size * boids->height; - pa->state.vel[2] = 0.0f; - bpa->data.mode = eBoidMode_OnLand; - } - /* fly above ground */ - else if (bpa->ground) { - pa->state.co[2] = ground_co[2] + pa->size * boids->height; - pa->state.vel[2] = 0.0f; - } - } - break; - } - case eBoidMode_Falling: - { - float grav[3]; - - grav[0] = 0.0f; - grav[1] = 0.0f; - grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f; - - - /* gather apparent gravity */ - madd_v3_v3fl(bpa->gravity, grav, dtime); - normalize_v3(bpa->gravity); - - if (boids->options & BOID_ALLOW_LAND) { - /* stick boid on goal when close enough */ - if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= pa->size * boids->height) { - bpa->data.mode = eBoidMode_Climbing; - bpa->ground = bbd->goal_ob; - boid_find_ground(bbd, pa, ground_co, ground_nor); - boid_climb(boids, pa, ground_co, ground_nor); - } - /* land boid when really near ground */ - else if (pa->state.co[2] <= ground_co[2] + 1.01f * pa->size * boids->height) { - pa->state.co[2] = ground_co[2] + pa->size * boids->height; - pa->state.vel[2] = 0.0f; - bpa->data.mode = eBoidMode_OnLand; - } - /* if we're falling, can fly and want to go upwards lets fly */ - else if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) - bpa->data.mode = eBoidMode_InAir; - } - else - bpa->data.mode = eBoidMode_InAir; - break; - } - case eBoidMode_Climbing: - { - boid_climb(boids, pa, ground_co, ground_nor); - //float nor[3]; - //copy_v3_v3(nor, ground_nor); - - ///* gather apparent gravity to r_ve */ - //madd_v3_v3fl(pa->r_ve, ground_nor, -1.0); - //normalize_v3(pa->r_ve); - - ///* raise boid it's size from surface */ - //mul_v3_fl(nor, pa->size * boids->height); - //add_v3_v3v3(pa->state.co, ground_co, nor); - - ///* remove normal component from velocity */ - //project_v3_v3v3(v, pa->state.vel, ground_nor); - //sub_v3_v3v3(pa->state.vel, pa->state.vel, v); - break; - } - case eBoidMode_OnLand: - { - /* stick boid on goal when close enough */ - if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= pa->size * boids->height) { - bpa->data.mode = eBoidMode_Climbing; - bpa->ground = bbd->goal_ob; - boid_find_ground(bbd, pa, ground_co, ground_nor); - boid_climb(boids, pa, ground_co, ground_nor); - } - /* ground is too far away so boid falls */ - else if (pa->state.co[2]-ground_co[2] > 1.1f * pa->size * boids->height) - bpa->data.mode = eBoidMode_Falling; - else { - /* constrain to surface */ - pa->state.co[2] = ground_co[2] + pa->size * boids->height; - pa->state.vel[2] = 0.0f; - } - - if (boids->banking > 0.0f) { - float grav[3]; - /* Don't take gravity's strength in to account, */ - /* otherwise amount of banking is hard to control. */ - negate_v3_v3(grav, ground_nor); - - project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel); - sub_v3_v3v3(dvec, bpa->data.acc, dvec); - - /* gather apparent gravity */ - madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking); - normalize_v3(bpa->gravity); - } - else { - /* gather negative surface normal */ - madd_v3_v3fl(bpa->gravity, ground_nor, -1.0f); - normalize_v3(bpa->gravity); - } - break; - } - } - - /* save direction to state.ave unless the boid is falling */ - /* (boids can't effect their direction when falling) */ - if (bpa->data.mode!=eBoidMode_Falling && len_v3(pa->state.vel) > 0.1f*pa->size) { - copy_v3_v3(pa->state.ave, pa->state.vel); - pa->state.ave[2] *= bbd->part->boids->pitch; - normalize_v3(pa->state.ave); - } - - /* apply damping */ - if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) - mul_v3_fl(pa->state.vel, 1.0f - 0.2f*bbd->part->dampfac); - - /* calculate rotation matrix based on forward & down vectors */ - if (bpa->data.mode == eBoidMode_InAir) { - copy_v3_v3(mat[0], pa->state.ave); - - project_v3_v3v3(dvec, bpa->gravity, pa->state.ave); - sub_v3_v3v3(mat[2], bpa->gravity, dvec); - normalize_v3(mat[2]); - } - else { - project_v3_v3v3(dvec, pa->state.ave, bpa->gravity); - sub_v3_v3v3(mat[0], pa->state.ave, dvec); - normalize_v3(mat[0]); - - copy_v3_v3(mat[2], bpa->gravity); - } - negate_v3(mat[2]); - cross_v3_v3v3(mat[1], mat[2], mat[0]); - - /* apply rotation */ - mat3_to_quat_is_ok(q, mat); - copy_qt_qt(pa->state.rot, q); + BoidSettings *boids = bbd->part->boids; + BoidParticle *bpa = pa->boid; + BoidValues val; + EffectedPoint epoint; + float acc[3] = {0.0f, 0.0f, 0.0f}, tan_acc[3], nor_acc[3]; + float dvec[3], bvec[3]; + float new_dir[3], new_speed; + float old_dir[3], old_speed; + float wanted_dir[3]; + float q[4], mat[3][3]; /* rotation */ + float ground_co[3] = {0.0f, 0.0f, 0.0f}, ground_nor[3] = {0.0f, 0.0f, 1.0f}; + float force[3] = {0.0f, 0.0f, 0.0f}; + float pa_mass = bbd->part->mass, dtime = bbd->dfra * bbd->timestep; + + set_boid_values(&val, boids, pa); + + /* make sure there's something in new velocity, location & rotation */ + copy_particle_key(&pa->state, &pa->prev_state, 0); + + if (bbd->part->flag & PART_SIZEMASS) + pa_mass *= pa->size; + + /* if boids can't fly they fall to the ground */ + if ((boids->options & BOID_ALLOW_FLIGHT) == 0 && + ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing) == 0 && + psys_uses_gravity(bbd->sim)) + bpa->data.mode = eBoidMode_Falling; + + if (bpa->data.mode == eBoidMode_Falling) { + /* Falling boids are only effected by gravity. */ + acc[2] = bbd->sim->scene->physics_settings.gravity[2]; + } + else { + /* figure out acceleration */ + float landing_level = 2.0f; + float level = landing_level + 1.0f; + float new_vel[3]; + + if (bpa->data.mode == eBoidMode_Liftoff) { + bpa->data.mode = eBoidMode_InAir; + bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); + } + else if (bpa->data.mode == eBoidMode_InAir && boids->options & BOID_ALLOW_LAND) { + /* auto-leveling & landing if close to ground */ + + bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); + + /* level = how many particle sizes above ground */ + level = (pa->prev_state.co[2] - ground_co[2]) / (2.0f * pa->size) - 0.5f; + + landing_level = -boids->landing_smoothness * pa->prev_state.vel[2] * pa_mass; + + if (pa->prev_state.vel[2] < 0.0f) { + if (level < 1.0f) { + bbd->wanted_co[0] = bbd->wanted_co[1] = bbd->wanted_co[2] = 0.0f; + bbd->wanted_speed = 0.0f; + bpa->data.mode = eBoidMode_Falling; + } + else if (level < landing_level) { + bbd->wanted_speed *= (level - 1.0f) / landing_level; + bbd->wanted_co[2] *= (level - 1.0f) / landing_level; + } + } + } + + copy_v3_v3(old_dir, pa->prev_state.ave); + new_speed = normalize_v3_v3(wanted_dir, bbd->wanted_co); + + /* first check if we have valid direction we want to go towards */ + if (new_speed == 0.0f) { + copy_v3_v3(new_dir, old_dir); + } + else { + float old_dir2[2], wanted_dir2[2], nor[3], angle; + copy_v2_v2(old_dir2, old_dir); + normalize_v2(old_dir2); + copy_v2_v2(wanted_dir2, wanted_dir); + normalize_v2(wanted_dir2); + + /* choose random direction to turn if wanted velocity */ + /* is directly behind regardless of z-coordinate */ + if (dot_v2v2(old_dir2, wanted_dir2) < -0.99f) { + wanted_dir[0] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + wanted_dir[1] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + wanted_dir[2] = 2.0f * (0.5f - BLI_rng_get_float(bbd->rng)); + normalize_v3(wanted_dir); + } + + /* constrain direction with maximum angular velocity */ + angle = saacos(dot_v3v3(old_dir, wanted_dir)); + angle = min_ff(angle, val.max_ave); + + cross_v3_v3v3(nor, old_dir, wanted_dir); + axis_angle_to_quat(q, nor, angle); + copy_v3_v3(new_dir, old_dir); + mul_qt_v3(q, new_dir); + normalize_v3(new_dir); + + /* save direction in case resulting velocity too small */ + axis_angle_to_quat(q, nor, angle * dtime); + copy_v3_v3(pa->state.ave, old_dir); + mul_qt_v3(q, pa->state.ave); + normalize_v3(pa->state.ave); + } + + /* constrain speed with maximum acceleration */ + old_speed = len_v3(pa->prev_state.vel); + + if (bbd->wanted_speed < old_speed) + new_speed = MAX2(bbd->wanted_speed, old_speed - val.max_acc); + else + new_speed = MIN2(bbd->wanted_speed, old_speed + val.max_acc); + + /* combine direction and speed */ + copy_v3_v3(new_vel, new_dir); + mul_v3_fl(new_vel, new_speed); + + /* maintain minimum flying velocity if not landing */ + if (level >= landing_level) { + float len2 = dot_v2v2(new_vel, new_vel); + float root; + + len2 = MAX2(len2, val.min_speed * val.min_speed); + root = sasqrt(new_speed * new_speed - len2); + + new_vel[2] = new_vel[2] < 0.0f ? -root : root; + + normalize_v2(new_vel); + mul_v2_fl(new_vel, sasqrt(len2)); + } + + /* finally constrain speed to max speed */ + new_speed = normalize_v3(new_vel); + mul_v3_fl(new_vel, MIN2(new_speed, val.max_speed)); + + /* get acceleration from difference of velocities */ + sub_v3_v3v3(acc, new_vel, pa->prev_state.vel); + + /* break acceleration to components */ + project_v3_v3v3(tan_acc, acc, pa->prev_state.ave); + sub_v3_v3v3(nor_acc, acc, tan_acc); + } + + /* account for effectors */ + pd_point_from_particle(bbd->sim, pa, &pa->state, &epoint); + BKE_effectors_apply(bbd->sim->psys->effectors, + bbd->sim->colliders, + bbd->part->effector_weights, + &epoint, + force, + NULL); + + if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) { + float length = normalize_v3(force); + + length = MAX2(0.0f, length - boids->land_stick_force); + + mul_v3_fl(force, length); + } + + add_v3_v3(acc, force); + + /* store smoothed acceleration for nice banking etc. */ + madd_v3_v3fl(bpa->data.acc, acc, dtime); + mul_v3_fl(bpa->data.acc, 1.0f / (1.0f + dtime)); + + /* integrate new location & velocity */ + + /* by regarding the acceleration as a force at this stage we + * can get better control although it's a bit unphysical */ + mul_v3_fl(acc, 1.0f / pa_mass); + + copy_v3_v3(dvec, acc); + mul_v3_fl(dvec, dtime * dtime * 0.5f); + + copy_v3_v3(bvec, pa->prev_state.vel); + mul_v3_fl(bvec, dtime); + add_v3_v3(dvec, bvec); + add_v3_v3(pa->state.co, dvec); + + madd_v3_v3fl(pa->state.vel, acc, dtime); + + //if (bpa->data.mode != eBoidMode_InAir) + bpa->ground = boid_find_ground(bbd, pa, ground_co, ground_nor); + + /* change modes, constrain movement & keep track of down vector */ + switch (bpa->data.mode) { + case eBoidMode_InAir: { + float grav[3]; + + grav[0] = 0.0f; + grav[1] = 0.0f; + grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f; + + /* don't take forward acceleration into account (better banking) */ + if (dot_v3v3(bpa->data.acc, pa->state.vel) > 0.0f) { + project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel); + sub_v3_v3v3(dvec, bpa->data.acc, dvec); + } + else { + copy_v3_v3(dvec, bpa->data.acc); + } + + /* gather apparent gravity */ + madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking); + normalize_v3(bpa->gravity); + + /* stick boid on goal when close enough */ + if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= + pa->size * boids->height) { + bpa->data.mode = eBoidMode_Climbing; + bpa->ground = bbd->goal_ob; + boid_find_ground(bbd, pa, ground_co, ground_nor); + boid_climb(boids, pa, ground_co, ground_nor); + } + else if (pa->state.co[2] <= ground_co[2] + pa->size * boids->height) { + /* land boid when below ground */ + if (boids->options & BOID_ALLOW_LAND) { + pa->state.co[2] = ground_co[2] + pa->size * boids->height; + pa->state.vel[2] = 0.0f; + bpa->data.mode = eBoidMode_OnLand; + } + /* fly above ground */ + else if (bpa->ground) { + pa->state.co[2] = ground_co[2] + pa->size * boids->height; + pa->state.vel[2] = 0.0f; + } + } + break; + } + case eBoidMode_Falling: { + float grav[3]; + + grav[0] = 0.0f; + grav[1] = 0.0f; + grav[2] = bbd->sim->scene->physics_settings.gravity[2] < 0.0f ? -1.0f : 0.0f; + + /* gather apparent gravity */ + madd_v3_v3fl(bpa->gravity, grav, dtime); + normalize_v3(bpa->gravity); + + if (boids->options & BOID_ALLOW_LAND) { + /* stick boid on goal when close enough */ + if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= + pa->size * boids->height) { + bpa->data.mode = eBoidMode_Climbing; + bpa->ground = bbd->goal_ob; + boid_find_ground(bbd, pa, ground_co, ground_nor); + boid_climb(boids, pa, ground_co, ground_nor); + } + /* land boid when really near ground */ + else if (pa->state.co[2] <= ground_co[2] + 1.01f * pa->size * boids->height) { + pa->state.co[2] = ground_co[2] + pa->size * boids->height; + pa->state.vel[2] = 0.0f; + bpa->data.mode = eBoidMode_OnLand; + } + /* if we're falling, can fly and want to go upwards lets fly */ + else if (boids->options & BOID_ALLOW_FLIGHT && bbd->wanted_co[2] > 0.0f) + bpa->data.mode = eBoidMode_InAir; + } + else + bpa->data.mode = eBoidMode_InAir; + break; + } + case eBoidMode_Climbing: { + boid_climb(boids, pa, ground_co, ground_nor); + //float nor[3]; + //copy_v3_v3(nor, ground_nor); + + ///* gather apparent gravity to r_ve */ + //madd_v3_v3fl(pa->r_ve, ground_nor, -1.0); + //normalize_v3(pa->r_ve); + + ///* raise boid it's size from surface */ + //mul_v3_fl(nor, pa->size * boids->height); + //add_v3_v3v3(pa->state.co, ground_co, nor); + + ///* remove normal component from velocity */ + //project_v3_v3v3(v, pa->state.vel, ground_nor); + //sub_v3_v3v3(pa->state.vel, pa->state.vel, v); + break; + } + case eBoidMode_OnLand: { + /* stick boid on goal when close enough */ + if (bbd->goal_ob && boid_goal_signed_dist(pa->state.co, bbd->goal_co, bbd->goal_nor) <= + pa->size * boids->height) { + bpa->data.mode = eBoidMode_Climbing; + bpa->ground = bbd->goal_ob; + boid_find_ground(bbd, pa, ground_co, ground_nor); + boid_climb(boids, pa, ground_co, ground_nor); + } + /* ground is too far away so boid falls */ + else if (pa->state.co[2] - ground_co[2] > 1.1f * pa->size * boids->height) + bpa->data.mode = eBoidMode_Falling; + else { + /* constrain to surface */ + pa->state.co[2] = ground_co[2] + pa->size * boids->height; + pa->state.vel[2] = 0.0f; + } + + if (boids->banking > 0.0f) { + float grav[3]; + /* Don't take gravity's strength in to account, */ + /* otherwise amount of banking is hard to control. */ + negate_v3_v3(grav, ground_nor); + + project_v3_v3v3(dvec, bpa->data.acc, pa->state.vel); + sub_v3_v3v3(dvec, bpa->data.acc, dvec); + + /* gather apparent gravity */ + madd_v3_v3v3fl(bpa->gravity, grav, dvec, -boids->banking); + normalize_v3(bpa->gravity); + } + else { + /* gather negative surface normal */ + madd_v3_v3fl(bpa->gravity, ground_nor, -1.0f); + normalize_v3(bpa->gravity); + } + break; + } + } + + /* save direction to state.ave unless the boid is falling */ + /* (boids can't effect their direction when falling) */ + if (bpa->data.mode != eBoidMode_Falling && len_v3(pa->state.vel) > 0.1f * pa->size) { + copy_v3_v3(pa->state.ave, pa->state.vel); + pa->state.ave[2] *= bbd->part->boids->pitch; + normalize_v3(pa->state.ave); + } + + /* apply damping */ + if (ELEM(bpa->data.mode, eBoidMode_OnLand, eBoidMode_Climbing)) + mul_v3_fl(pa->state.vel, 1.0f - 0.2f * bbd->part->dampfac); + + /* calculate rotation matrix based on forward & down vectors */ + if (bpa->data.mode == eBoidMode_InAir) { + copy_v3_v3(mat[0], pa->state.ave); + + project_v3_v3v3(dvec, bpa->gravity, pa->state.ave); + sub_v3_v3v3(mat[2], bpa->gravity, dvec); + normalize_v3(mat[2]); + } + else { + project_v3_v3v3(dvec, pa->state.ave, bpa->gravity); + sub_v3_v3v3(mat[0], pa->state.ave, dvec); + normalize_v3(mat[0]); + + copy_v3_v3(mat[2], bpa->gravity); + } + negate_v3(mat[2]); + cross_v3_v3v3(mat[1], mat[2], mat[0]); + + /* apply rotation */ + mat3_to_quat_is_ok(q, mat); + copy_qt_qt(pa->state.rot, q); } BoidRule *boid_new_rule(int type) { - BoidRule *rule = NULL; - if (type <= 0) - return NULL; - - switch (type) { - case eBoidRuleType_Goal: - case eBoidRuleType_Avoid: - rule = MEM_callocN(sizeof(BoidRuleGoalAvoid), "BoidRuleGoalAvoid"); - break; - case eBoidRuleType_AvoidCollision: - rule = MEM_callocN(sizeof(BoidRuleAvoidCollision), "BoidRuleAvoidCollision"); - ((BoidRuleAvoidCollision*)rule)->look_ahead = 2.0f; - break; - case eBoidRuleType_FollowLeader: - rule = MEM_callocN(sizeof(BoidRuleFollowLeader), "BoidRuleFollowLeader"); - ((BoidRuleFollowLeader*)rule)->distance = 1.0f; - break; - case eBoidRuleType_AverageSpeed: - rule = MEM_callocN(sizeof(BoidRuleAverageSpeed), "BoidRuleAverageSpeed"); - ((BoidRuleAverageSpeed*)rule)->speed = 0.5f; - break; - case eBoidRuleType_Fight: - rule = MEM_callocN(sizeof(BoidRuleFight), "BoidRuleFight"); - ((BoidRuleFight*)rule)->distance = 100.0f; - ((BoidRuleFight*)rule)->flee_distance = 100.0f; - break; - default: - rule = MEM_callocN(sizeof(BoidRule), "BoidRule"); - break; - } - - rule->type = type; - rule->flag |= BOIDRULE_IN_AIR|BOIDRULE_ON_LAND; - BLI_strncpy(rule->name, rna_enum_boidrule_type_items[type-1].name, sizeof(rule->name)); - - return rule; + BoidRule *rule = NULL; + if (type <= 0) + return NULL; + + switch (type) { + case eBoidRuleType_Goal: + case eBoidRuleType_Avoid: + rule = MEM_callocN(sizeof(BoidRuleGoalAvoid), "BoidRuleGoalAvoid"); + break; + case eBoidRuleType_AvoidCollision: + rule = MEM_callocN(sizeof(BoidRuleAvoidCollision), "BoidRuleAvoidCollision"); + ((BoidRuleAvoidCollision *)rule)->look_ahead = 2.0f; + break; + case eBoidRuleType_FollowLeader: + rule = MEM_callocN(sizeof(BoidRuleFollowLeader), "BoidRuleFollowLeader"); + ((BoidRuleFollowLeader *)rule)->distance = 1.0f; + break; + case eBoidRuleType_AverageSpeed: + rule = MEM_callocN(sizeof(BoidRuleAverageSpeed), "BoidRuleAverageSpeed"); + ((BoidRuleAverageSpeed *)rule)->speed = 0.5f; + break; + case eBoidRuleType_Fight: + rule = MEM_callocN(sizeof(BoidRuleFight), "BoidRuleFight"); + ((BoidRuleFight *)rule)->distance = 100.0f; + ((BoidRuleFight *)rule)->flee_distance = 100.0f; + break; + default: + rule = MEM_callocN(sizeof(BoidRule), "BoidRule"); + break; + } + + rule->type = type; + rule->flag |= BOIDRULE_IN_AIR | BOIDRULE_ON_LAND; + BLI_strncpy(rule->name, rna_enum_boidrule_type_items[type - 1].name, sizeof(rule->name)); + + return rule; } void boid_default_settings(BoidSettings *boids) { - boids->air_max_speed = 10.0f; - boids->air_max_acc = 0.5f; - boids->air_max_ave = 0.5f; - boids->air_personal_space = 1.0f; - - boids->land_max_speed = 5.0f; - boids->land_max_acc = 0.5f; - boids->land_max_ave = 0.5f; - boids->land_personal_space = 1.0f; - - boids->options = BOID_ALLOW_FLIGHT; - - boids->landing_smoothness = 3.0f; - boids->banking = 1.0f; - boids->pitch = 1.0f; - boids->height = 1.0f; - - boids->health = 1.0f; - boids->accuracy = 1.0f; - boids->aggression = 2.0f; - boids->range = 1.0f; - boids->strength = 0.1f; + boids->air_max_speed = 10.0f; + boids->air_max_acc = 0.5f; + boids->air_max_ave = 0.5f; + boids->air_personal_space = 1.0f; + + boids->land_max_speed = 5.0f; + boids->land_max_acc = 0.5f; + boids->land_max_ave = 0.5f; + boids->land_personal_space = 1.0f; + + boids->options = BOID_ALLOW_FLIGHT; + + boids->landing_smoothness = 3.0f; + boids->banking = 1.0f; + boids->pitch = 1.0f; + boids->height = 1.0f; + + boids->health = 1.0f; + boids->accuracy = 1.0f; + boids->aggression = 2.0f; + boids->range = 1.0f; + boids->strength = 0.1f; } BoidState *boid_new_state(BoidSettings *boids) { - BoidState *state = MEM_callocN(sizeof(BoidState), "BoidState"); + BoidState *state = MEM_callocN(sizeof(BoidState), "BoidState"); - state->id = boids->last_state_id++; - if (state->id) - BLI_snprintf(state->name, sizeof(state->name), "State %i", state->id); - else - strcpy(state->name, "State"); + state->id = boids->last_state_id++; + if (state->id) + BLI_snprintf(state->name, sizeof(state->name), "State %i", state->id); + else + strcpy(state->name, "State"); - state->rule_fuzziness = 0.5; - state->volume = 1.0f; - state->channels |= ~0; + state->rule_fuzziness = 0.5; + state->volume = 1.0f; + state->channels |= ~0; - return state; + return state; } BoidState *boid_duplicate_state(BoidSettings *boids, BoidState *state) { - BoidState *staten = MEM_dupallocN(state); + BoidState *staten = MEM_dupallocN(state); - BLI_duplicatelist(&staten->rules, &state->rules); - BLI_duplicatelist(&staten->conditions, &state->conditions); - BLI_duplicatelist(&staten->actions, &state->actions); + BLI_duplicatelist(&staten->rules, &state->rules); + BLI_duplicatelist(&staten->conditions, &state->conditions); + BLI_duplicatelist(&staten->actions, &state->actions); - staten->id = boids->last_state_id++; + staten->id = boids->last_state_id++; - return staten; + return staten; } void boid_free_settings(BoidSettings *boids) { - if (boids) { - BoidState *state = boids->states.first; + if (boids) { + BoidState *state = boids->states.first; - for (; state; state=state->next) { - BLI_freelistN(&state->rules); - BLI_freelistN(&state->conditions); - BLI_freelistN(&state->actions); - } + for (; state; state = state->next) { + BLI_freelistN(&state->rules); + BLI_freelistN(&state->conditions); + BLI_freelistN(&state->actions); + } - BLI_freelistN(&boids->states); + BLI_freelistN(&boids->states); - MEM_freeN(boids); - } + MEM_freeN(boids); + } } BoidSettings *boid_copy_settings(const BoidSettings *boids) { - BoidSettings *nboids = NULL; + BoidSettings *nboids = NULL; - if (boids) { - BoidState *state; - BoidState *nstate; + if (boids) { + BoidState *state; + BoidState *nstate; - nboids = MEM_dupallocN(boids); + nboids = MEM_dupallocN(boids); - BLI_duplicatelist(&nboids->states, &boids->states); + BLI_duplicatelist(&nboids->states, &boids->states); - state = boids->states.first; - nstate = nboids->states.first; - for (; state; state=state->next, nstate=nstate->next) { - BLI_duplicatelist(&nstate->rules, &state->rules); - BLI_duplicatelist(&nstate->conditions, &state->conditions); - BLI_duplicatelist(&nstate->actions, &state->actions); - } - } + state = boids->states.first; + nstate = nboids->states.first; + for (; state; state = state->next, nstate = nstate->next) { + BLI_duplicatelist(&nstate->rules, &state->rules); + BLI_duplicatelist(&nstate->conditions, &state->conditions); + BLI_duplicatelist(&nstate->actions, &state->actions); + } + } - return nboids; + return nboids; } BoidState *boid_get_current_state(BoidSettings *boids) { - BoidState *state = boids->states.first; + BoidState *state = boids->states.first; - for (; state; state=state->next) { - if (state->flag & BOIDSTATE_CURRENT) - break; - } + for (; state; state = state->next) { + if (state->flag & BOIDSTATE_CURRENT) + break; + } - return state; + return state; } |