/*
* Shrinkwrap moving vertexs to the nearest surface point on the target
*
* it builds a BVHTree from the target mesh and then performs a
* NN matches for each vertex
*/
static void shrinkwrap_calc_nearest_surface_point(ShrinkwrapCalcData *calc)
{
int i;
BVHTreeFromMesh treeData = NULL_BVHTreeFromMesh;
BVHTreeNearest nearest = NULL_BVHTreeNearest;
/* Create a bvh-tree of the given target */
bvhtree_from_mesh_faces(&treeData, calc->target, 0.0, 2, 6);
if (treeData.tree == NULL) {
OUT_OF_MEMORY();
return;
}
/* Setup nearest */
nearest.index = -1;
nearest.dist_sq = FLT_MAX;
/* Find the nearest vertex */
#ifndef __APPLE__
#pragma omp parallel for default(none) private(i) firstprivate(nearest) shared(calc, treeData) schedule(static)
#endif
for (i = 0; i < calc->numVerts; ++i) {
float *co = calc->vertexCos[i];
float tmp_co[3];
float weight = defvert_array_find_weight_safe(calc->dvert, i, calc->vgroup);
if (weight == 0.0f) continue;
/* Convert the vertex to tree coordinates */
if (calc->vert) {
copy_v3_v3(tmp_co, calc->vert[i].co);
}
else {
copy_v3_v3(tmp_co, co);
}
space_transform_apply(&calc->local2target, tmp_co);
/* Use local proximity heuristics (to reduce the nearest search)
*
* If we already had an hit before.. we assume this vertex is going to have a close hit to that other vertex
* so we can initiate the "nearest.dist" with the expected value to that last hit.
* This will lead in pruning of the search tree. */
if (nearest.index != -1)
nearest.dist_sq = len_squared_v3v3(tmp_co, nearest.co);
else
nearest.dist_sq = FLT_MAX;
BLI_bvhtree_find_nearest(treeData.tree, tmp_co, &nearest, treeData.nearest_callback, &treeData);
/* Found the nearest vertex */
if (nearest.index != -1) {
if (calc->smd->shrinkOpts & MOD_SHRINKWRAP_KEEP_ABOVE_SURFACE) {
/* Make the vertex stay on the front side of the face */
madd_v3_v3v3fl(tmp_co, nearest.co, nearest.no, calc->keepDist);
}
else {
/* Adjusting the vertex weight,
* so that after interpolating it keeps a certain distance from the nearest position */
const float dist = sasqrt(nearest.dist_sq);
if (dist > FLT_EPSILON) {
/* linear interpolation */
interp_v3_v3v3(tmp_co, tmp_co, nearest.co, (dist - calc->keepDist) / dist);
}
else {
copy_v3_v3(tmp_co, nearest.co);
}
}
/* Convert the coordinates back to mesh coordinates */
space_transform_invert(&calc->local2target, tmp_co);
interp_v3_v3v3(co, co, tmp_co, weight); /* linear interpolation */
}
}
free_bvhtree_from_mesh(&treeData);
}
/* 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) */
rule = BLI_findlink(&state->rules, rand % BLI_listbase_count(&state->rules));
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) {
}
if (jump) {
copy_v3_v3(pa->prev_state.vel, jump_v);
bpa->data.mode = eBoidMode_Falling;
}
}
}
}
/**
* \param col (float[4]) Store the rgb color here (with alpha)
* The alpha is used to blend the color to the background
* color_new = (1-alpha)*color_background + color
* \param zz The current zbuffer value at the place of this pixel
* \param dist Distance of the pixel from the center of the halo squared. Given in pixels
* \param xn The x coordinate of the pixel relaticve to the center of the halo. given in pixels
* \param yn The y coordinate of the pixel relaticve to the center of the halo. given in pixels
*/
int shadeHaloFloat(HaloRen *har, float col[4], int zz,
float dist, float xn, float yn, short flarec)
{
/* fill in col */
float t, zn, radist, ringf=0.0f, linef=0.0f, alpha, si, co;
int a;
if (R.wrld.mode & WO_MIST) {
if (har->type & HA_ONLYSKY) {
alpha= har->alfa;
}
else {
/* a bit patchy... */
alpha= mistfactor(-har->co[2], har->co)*har->alfa;
}
}
else alpha= har->alfa;
if (alpha==0.0f)
return 0;
/* soften the halo if it intersects geometry */
if (har->mat && har->mat->mode & MA_HALO_SOFT) {
float segment_length, halo_depth, distance_from_z /* , visible_depth */ /* UNUSED */, soften;
/* calculate halo depth */
segment_length= har->hasize*sasqrt(1.0f - dist/(har->rad*har->rad));
halo_depth= 2.0f*segment_length;
if (halo_depth < FLT_EPSILON)
return 0;
/* calculate how much of this depth is visible */
distance_from_z = haloZtoDist(zz) - haloZtoDist(har->zs);
/* visible_depth = halo_depth; */ /* UNUSED */
if (distance_from_z < segment_length) {
soften= (segment_length + distance_from_z)/halo_depth;
/* apply softening to alpha */
if (soften < 1.0f)
alpha *= soften;
if (alpha <= 0.0f)
return 0;
}
}
else {
/* not a soft halo. use the old softening code */
/* halo being intersected? */
if (har->zs> zz-har->zd) {
t= ((float)(zz-har->zs))/(float)har->zd;
alpha*= sqrtf(sqrtf(t));
}
}
radist = sqrtf(dist);
/* watch it: not used nicely: flarec is set at zero in pixstruct */
if (flarec) har->pixels+= (int)(har->rad-radist);
if (har->ringc) {
const float *rc;
float fac;
int ofs;
/* per ring an antialised circle */
ofs= har->seed;
for (a= har->ringc; a>0; a--, ofs+=2) {
rc= hashvectf + (ofs % 768);
fac = fabsf(rc[1] * (har->rad * fabsf(rc[0]) - radist));
if (fac< 1.0f) {
ringf+= (1.0f-fac);
}
}
}
if (har->type & HA_VECT) {
dist= fabsf(har->cos * (yn) - har->sin * (xn)) / har->rad;
if (dist>1.0f) dist= 1.0f;
if (har->tex) {
zn= har->sin*xn - har->cos*yn;
yn= har->cos*xn + har->sin*yn;
xn= zn;
}
}
else dist= dist/har->radsq;
if (har->type & HA_FLARECIRC) {
dist = 0.5f + fabsf(dist - 0.5f);
}
if (har->hard>=30) {
dist = sqrtf(dist);
if (har->hard>=40) {
dist = sinf(dist*(float)M_PI_2);
if (har->hard>=50) {
dist = sqrtf(dist);
}
}
}
else if (har->hard<20) dist*=dist;
if (dist < 1.0f)
dist= (1.0f-dist);
else
dist= 0.0f;
if (har->linec) {
const float *rc;
float fac;
int ofs;
/* per starpoint an antialiased line */
ofs= har->seed;
for (a= har->linec; a>0; a--, ofs+=3) {
rc= hashvectf + (ofs % 768);
fac = fabsf((xn) * rc[0] + (yn) * rc[1]);
if (fac< 1.0f )
linef+= (1.0f-fac);
}
linef*= dist;
}
if (har->starpoints) {
float ster, angle;
/* rotation */
angle = atan2f(yn, xn);
angle *= (1.0f+0.25f*har->starpoints);
co= cosf(angle);
si= sinf(angle);
angle= (co*xn+si*yn)*(co*yn-si*xn);
ster = fabsf(angle);
if (ster>1.0f) {
ster= (har->rad)/(ster);
if (ster<1.0f) dist*= sqrtf(ster);
}
}
/* disputable optimize... (ton) */
if (dist<=0.00001f)
return 0;
dist*= alpha;
ringf*= dist;
linef*= alpha;
/* The color is either the rgb spec-ed by the user, or extracted from */
/* the texture */
if (har->tex) {
col[0]= har->r;
col[1]= har->g;
col[2]= har->b;
col[3]= dist;
do_halo_tex(har, xn, yn, col);
col[0]*= col[3];
col[1]*= col[3];
col[2]*= col[3];
}
else {
col[0]= dist*har->r;
col[1]= dist*har->g;
col[2]= dist*har->b;
if (har->type & HA_XALPHA) col[3]= dist*dist;
else col[3]= dist;
}
if (har->mat) {
if (har->mat->mode & MA_HALO_SHADE) {
/* we test for lights because of preview... */
if (R.lights.first) render_lighting_halo(har, col);
}
/* Next, we do the line and ring factor modifications. */
if (linef!=0.0f) {
Material *ma= har->mat;
col[0]+= linef * ma->specr;
col[1]+= linef * ma->specg;
col[2]+= linef * ma->specb;
if (har->type & HA_XALPHA) col[3]+= linef*linef;
else col[3]+= linef;
}
if (ringf!=0.0f) {
Material *ma= har->mat;
col[0]+= ringf * ma->mirr;
col[1]+= ringf * ma->mirg;
col[2]+= ringf * ma->mirb;
if (har->type & HA_XALPHA) col[3]+= ringf*ringf;
else col[3]+= ringf;
}
}
/* alpha requires clip, gives black dots */
if (col[3] > 1.0f)
col[3]= 1.0f;
return 1;
}
/* 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);
pdDoEffectors(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 allthough 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);
}
/*
* Shrinkwrap to the nearest vertex
*
* it builds a kdtree of vertexs we can attach to and then
* for each vertex performs a nearest vertex search on the tree
*/
static void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
{
int i;
BVHTreeFromMesh treeData = NULL_BVHTreeFromMesh;
BVHTreeNearest nearest = NULL_BVHTreeNearest;
BENCH(bvhtree_from_mesh_verts(&treeData, calc->target, 0.0, 2, 6));
if (treeData.tree == NULL) {
OUT_OF_MEMORY();
return;
}
//Setup nearest
nearest.index = -1;
nearest.dist = FLT_MAX;
#ifndef __APPLE__
#pragma omp parallel for default(none) private(i) firstprivate(nearest) shared(treeData,calc) schedule(static)
#endif
for (i = 0; i<calc->numVerts; ++i) {
float *co = calc->vertexCos[i];
float tmp_co[3];
float weight = defvert_array_find_weight_safe(calc->dvert, i, calc->vgroup);
if (weight == 0.0f) continue;
//Convert the vertex to tree coordinates
if (calc->vert) {
copy_v3_v3(tmp_co, calc->vert[i].co);
}
else {
copy_v3_v3(tmp_co, co);
}
space_transform_apply(&calc->local2target, tmp_co);
//Use local proximity heuristics (to reduce the nearest search)
//
//If we already had an hit before.. we assume this vertex is going to have a close hit to that other vertex
//so we can initiate the "nearest.dist" with the expected value to that last hit.
//This will lead in prunning of the search tree.
if (nearest.index != -1)
nearest.dist = len_squared_v3v3(tmp_co, nearest.co);
else
nearest.dist = FLT_MAX;
BLI_bvhtree_find_nearest(treeData.tree, tmp_co, &nearest, treeData.nearest_callback, &treeData);
//Found the nearest vertex
if (nearest.index != -1) {
//Adjusting the vertex weight, so that after interpolating it keeps a certain distance from the nearest position
float dist = sasqrt(nearest.dist);
if (dist > FLT_EPSILON) weight *= (dist - calc->keepDist)/dist;
//Convert the coordinates back to mesh coordinates
copy_v3_v3(tmp_co, nearest.co);
space_transform_invert(&calc->local2target, tmp_co);
interp_v3_v3v3(co, co, tmp_co, weight); //linear interpolation
}
}
free_bvhtree_from_mesh(&treeData);
}
