Ejemplo n.º 1
0
/* Return the shortest angle in radians between the 2 vectors */
float angle_v3v3(const float v1[3], const float v2[3])
{
	float vec1[3], vec2[3];

	normalize_v3_v3(vec1, v1);
	normalize_v3_v3(vec2, v2);

	return angle_normalized_v3v3(vec1, vec2);
}
Ejemplo n.º 2
0
/* shade sky according to sun lamps, all parameters are like shadeSkyView except sunsky*/
void shadeSunView(float col_r[3], const float view[3])
{
	GroupObject *go;
	LampRen *lar;
	float sview[3];
	bool do_init = true;
	
	for (go=R.lights.first; go; go= go->next) {
		lar= go->lampren;
		if (lar->type==LA_SUN &&	lar->sunsky && (lar->sunsky->effect_type & LA_SUN_EFFECT_SKY)) {
			float sun_collector[3];
			float colorxyz[3];
			
			if (do_init) {

				normalize_v3_v3(sview, view);
				mul_m3_v3(R.imat, sview);
				if (sview[2] < 0.0f)
					sview[2] = 0.0f;
				normalize_v3(sview);
				do_init = false;
			}
			
			GetSkyXYZRadiancef(lar->sunsky, sview, colorxyz);
			xyz_to_rgb(colorxyz[0], colorxyz[1], colorxyz[2], &sun_collector[0], &sun_collector[1], &sun_collector[2],
			           lar->sunsky->sky_colorspace);
			
			ramp_blend(lar->sunsky->skyblendtype, col_r, lar->sunsky->skyblendfac, sun_collector);
		}
	}
}
Ejemplo n.º 3
0
static void viewAxisCorrectCenter(TransInfo *t, float t_con_center[3])
{
	if (t->spacetype == SPACE_VIEW3D) {
		// View3D *v3d = t->sa->spacedata.first;
		const float min_dist = 1.0f;  /* v3d->near; */
		float dir[3];
		float l;

		sub_v3_v3v3(dir, t_con_center, t->viewinv[3]);
		if (dot_v3v3(dir, t->viewinv[2]) < 0.0f) {
			negate_v3(dir);
		}
		project_v3_v3v3(dir, dir, t->viewinv[2]);

		l = len_v3(dir);

		if (l < min_dist) {
			float diff[3];
			normalize_v3_v3(diff, t->viewinv[2]);
			mul_v3_fl(diff, min_dist - l);

			sub_v3_v3(t_con_center, diff);
		}
	}
}
Ejemplo n.º 4
0
void view3d_align_axis_to_vector(View3D *v3d, RegionView3D *rv3d, int axisidx, float vec[3])
{
	float alignaxis[3] = {0.0, 0.0, 0.0};
	float norm[3], axis[3], angle, new_quat[4];
	
	if(axisidx > 0) alignaxis[axisidx-1]= 1.0;
	else alignaxis[-axisidx-1]= -1.0;

	normalize_v3_v3(norm, vec);

	angle= (float)acos(dot_v3v3(alignaxis, norm));
	cross_v3_v3v3(axis, alignaxis, norm);
	axis_angle_to_quat( new_quat,axis, -angle);
	
	rv3d->view= RV3D_VIEW_USER;
	
	if (rv3d->persp==RV3D_CAMOB && v3d->camera) {
		/* switch out of camera view */
		float orig_ofs[3];
		float orig_dist= rv3d->dist;
		float orig_lens= v3d->lens;
		
		copy_v3_v3(orig_ofs, rv3d->ofs);
		rv3d->persp= RV3D_PERSP;
		rv3d->dist= 0.0;
		ED_view3d_from_object(v3d->camera, rv3d->ofs, NULL, NULL, &v3d->lens);
		smooth_view(NULL, NULL, NULL, NULL, NULL, orig_ofs, new_quat, &orig_dist, &orig_lens); // XXX
	} else {
		if (rv3d->persp==RV3D_CAMOB) rv3d->persp= RV3D_PERSP; /* switch out of camera mode */
		smooth_view(NULL, NULL, NULL, NULL, NULL, NULL, new_quat, NULL, NULL); // XXX
	}
}
Ejemplo n.º 5
0
void rotate_v3_v3v3fl(float r[3], const float p[3], const float axis[3], const float angle)
{
	float axis_n[3];

	normalize_v3_v3(axis_n, axis);

	rotate_normalized_v3_v3v3fl(r, p, axis_n, angle);
}
Ejemplo n.º 6
0
void GeometryExporter::create_normals(std::vector<Normal> &normals, std::vector<BCPolygonNormalsIndices> &polygons_normals, Mesh *me)
{
	std::map<Normal, unsigned int> shared_normal_indices;
	int last_normal_index = -1;

	MVert *verts  = me->mvert;
	MLoop *mloops = me->mloop;
	float(*lnors)[3];

	BKE_mesh_calc_normals_split(me);
	if (CustomData_has_layer(&me->ldata, CD_NORMAL)) {
		lnors = (float(*)[3])CustomData_get_layer(&me->ldata, CD_NORMAL);
	}

	for (int poly_index = 0; poly_index < me->totpoly; poly_index++) {
		MPoly *mpoly  = &me->mpoly[poly_index];

		if (!(mpoly->flag & ME_SMOOTH)) {
			// For flat faces use face normal as vertex normal:

			float vector[3];
			BKE_mesh_calc_poly_normal(mpoly, mloops+mpoly->loopstart, verts, vector);

			Normal n = { vector[0], vector[1], vector[2] };
			normals.push_back(n);
			last_normal_index++;
		}

		MLoop *mloop = mloops + mpoly->loopstart;
		BCPolygonNormalsIndices poly_indices;
		for (int loop_index = 0; loop_index < mpoly->totloop; loop_index++) {
			unsigned int loop_idx = mpoly->loopstart + loop_index;
			if (mpoly->flag & ME_SMOOTH) {

				float normalized[3];
				normalize_v3_v3(normalized, lnors[loop_idx]);
				Normal n = { normalized[0], normalized[1], normalized[2] };

				if (shared_normal_indices.find(n) != shared_normal_indices.end()) {
					poly_indices.add_index(shared_normal_indices[n]);
				}
				else {
					last_normal_index++;
					poly_indices.add_index(last_normal_index);
					shared_normal_indices[n] = last_normal_index;
					normals.push_back(n);
				}
			}
			else {
				poly_indices.add_index(last_normal_index);
			}
		}

		polygons_normals.push_back(poly_indices);
	}
}
Ejemplo n.º 7
0
static void camera_frame_fit_data_init(
        const Scene *scene, const Object *ob,
        CameraParams *params, CameraViewFrameData *data)
{
	float camera_rotmat_transposed_inversed[4][4];
	unsigned int i;

	/* setup parameters */
	BKE_camera_params_init(params);
	BKE_camera_params_from_object(params, ob);

	/* compute matrix, viewplane, .. */
	if (scene) {
		BKE_camera_params_compute_viewplane(params, scene->r.xsch, scene->r.ysch, scene->r.xasp, scene->r.yasp);
	}
	else {
		BKE_camera_params_compute_viewplane(params, 1, 1, 1.0f, 1.0f);
	}
	BKE_camera_params_compute_matrix(params);

	/* initialize callback data */
	copy_m3_m4(data->camera_rotmat, (float (*)[4])ob->obmat);
	normalize_m3(data->camera_rotmat);
	/* To transform a plane which is in its homogeneous representation (4d vector),
	 * we need the inverse of the transpose of the transform matrix... */
	copy_m4_m3(camera_rotmat_transposed_inversed, data->camera_rotmat);
	transpose_m4(camera_rotmat_transposed_inversed);
	invert_m4(camera_rotmat_transposed_inversed);

	/* Extract frustum planes from projection matrix. */
	planes_from_projmat(params->winmat,
	                    /*   left              right                 top              bottom        near  far */
	                    data->plane_tx[2], data->plane_tx[0], data->plane_tx[3], data->plane_tx[1], NULL, NULL);

	/* Rotate planes and get normals from them */
	for (i = 0; i < CAMERA_VIEWFRAME_NUM_PLANES; i++) {
		mul_m4_v4(camera_rotmat_transposed_inversed, data->plane_tx[i]);
		normalize_v3_v3(data->normal_tx[i], data->plane_tx[i]);
	}

	copy_v4_fl(data->dist_vals_sq, FLT_MAX);
	data->tot = 0;
	data->is_ortho = params->is_ortho;
	if (params->is_ortho) {
		/* we want (0, 0, -1) transformed by camera_rotmat, this is a quicker shortcut. */
		negate_v3_v3(data->camera_no, data->camera_rotmat[2]);
		data->dist_to_cam = FLT_MAX;
	}
}
Ejemplo n.º 8
0
void sk_initPoint(SK_Point *pt, SK_DrawData *dd, float *no)
{
	if (no)
	{
		normalize_v3_v3(pt->no, no);
	}
	else
	{
		pt->no[0] = 0;
		pt->no[1] = 0;
		pt->no[2] = 1;
	}
	pt->p2d[0] = dd->mval[0];
	pt->p2d[1] = dd->mval[1];
	/* more init code here */
}
Ejemplo n.º 9
0
/* get the camera's dof value, takes the dof object into account */
float BKE_camera_object_dof_distance(Object *ob)
{
	Camera *cam = (Camera *)ob->data;
	if (ob->type != OB_CAMERA)
		return 0.0f;
	if (cam->dof_ob) {
#if 0
		/* too simple, better to return the distance on the view axis only */
		return len_v3v3(ob->obmat[3], cam->dof_ob->obmat[3]);
#else
		float view_dir[3], dof_dir[3];
		normalize_v3_v3(view_dir, ob->obmat[2]);
		sub_v3_v3v3(dof_dir, ob->obmat[3], cam->dof_ob->obmat[3]);
		return fabsf(dot_v3v3(view_dir, dof_dir));
#endif
	}
	return cam->YF_dofdist;
}
Ejemplo n.º 10
0
void sk_initPoint(SK_Point *pt, SK_DrawData *dd, const float no[3])
{
	if (no) {
		normalize_v3_v3(pt->no, no);
	}
	else {
		pt->no[0] = 0.0f;
		pt->no[1] = 0.0f;
		pt->no[2] = 1.0f;
	}
	pt->p2d[0] = dd->mval[0];
	pt->p2d[1] = dd->mval[1];

	pt->size = 0.0f;
	pt->type = PT_CONTINUOUS;
	pt->mode = PT_SNAP;
	/* more init code here */
}
Ejemplo n.º 11
0
void strand_eval_point(StrandSegment *sseg, StrandPoint *spoint)
{
	Material *ma;
	StrandBuffer *strandbuf;
	float *simplify;
	float p[4][3], data[4], cross[3], w, dx, dy, t;
	int type;

	strandbuf= sseg->buffer;
	ma= sseg->buffer->ma;
	t= spoint->t;
	type= (strandbuf->flag & R_STRAND_BSPLINE)? KEY_BSPLINE: KEY_CARDINAL;

	copy_v3_v3(p[0], sseg->v[0]->co);
	copy_v3_v3(p[1], sseg->v[1]->co);
	copy_v3_v3(p[2], sseg->v[2]->co);
	copy_v3_v3(p[3], sseg->v[3]->co);

	if (sseg->obi->flag & R_TRANSFORMED) {
		mul_m4_v3(sseg->obi->mat, p[0]);
		mul_m4_v3(sseg->obi->mat, p[1]);
		mul_m4_v3(sseg->obi->mat, p[2]);
		mul_m4_v3(sseg->obi->mat, p[3]);
	}

	if (t == 0.0f) {
		copy_v3_v3(spoint->co, p[1]);
		spoint->strandco= sseg->v[1]->strandco;

		spoint->dtstrandco= (sseg->v[2]->strandco - sseg->v[0]->strandco);
		if (sseg->v[0] != sseg->v[1])
			spoint->dtstrandco *= 0.5f;
	}
	else if (t == 1.0f) {
		copy_v3_v3(spoint->co, p[2]);
		spoint->strandco= sseg->v[2]->strandco;

		spoint->dtstrandco= (sseg->v[3]->strandco - sseg->v[1]->strandco);
		if (sseg->v[3] != sseg->v[2])
			spoint->dtstrandco *= 0.5f;
	}
	else {
		key_curve_position_weights(t, data, type);
		spoint->co[0]= data[0]*p[0][0] + data[1]*p[1][0] + data[2]*p[2][0] + data[3]*p[3][0];
		spoint->co[1]= data[0]*p[0][1] + data[1]*p[1][1] + data[2]*p[2][1] + data[3]*p[3][1];
		spoint->co[2]= data[0]*p[0][2] + data[1]*p[1][2] + data[2]*p[2][2] + data[3]*p[3][2];
		spoint->strandco= (1.0f-t)*sseg->v[1]->strandco + t*sseg->v[2]->strandco;
	}

	key_curve_tangent_weights(t, data, type);
	spoint->dtco[0]= data[0]*p[0][0] + data[1]*p[1][0] + data[2]*p[2][0] + data[3]*p[3][0];
	spoint->dtco[1]= data[0]*p[0][1] + data[1]*p[1][1] + data[2]*p[2][1] + data[3]*p[3][1];
	spoint->dtco[2]= data[0]*p[0][2] + data[1]*p[1][2] + data[2]*p[2][2] + data[3]*p[3][2];

	normalize_v3_v3(spoint->tan, spoint->dtco);
	normalize_v3_v3(spoint->nor, spoint->co);
	negate_v3(spoint->nor);

	spoint->width= strand_eval_width(ma, spoint->strandco);
	
	/* simplification */
	simplify= RE_strandren_get_simplify(strandbuf->obr, sseg->strand, 0);
	spoint->alpha= (simplify)? simplify[1]: 1.0f;

	/* outer points */
	cross_v3_v3v3(cross, spoint->co, spoint->tan);

	w= spoint->co[2]*strandbuf->winmat[2][3] + strandbuf->winmat[3][3];
	dx= strandbuf->winx*cross[0]*strandbuf->winmat[0][0]/w;
	dy= strandbuf->winy*cross[1]*strandbuf->winmat[1][1]/w;
	w= sqrt(dx*dx + dy*dy);

	if (w > 0.0f) {
		if (strandbuf->flag & R_STRAND_B_UNITS) {
			const float crosslen= len_v3(cross);
			w= 2.0f*crosslen*strandbuf->minwidth/w;

			if (spoint->width < w) {
				spoint->alpha= spoint->width/w;
				spoint->width= w;
			}

			if (simplify)
				/* squared because we only change width, not length */
				spoint->width *= simplify[0]*simplify[0];

			mul_v3_fl(cross, spoint->width*0.5f/crosslen);
		}
		else
			mul_v3_fl(cross, spoint->width/w);
	}

	sub_v3_v3v3(spoint->co1, spoint->co, cross);
	add_v3_v3v3(spoint->co2, spoint->co, cross);

	copy_v3_v3(spoint->dsco, cross);
}
Ejemplo n.º 12
0
void do_kink(ParticleKey *state, const float par_co[3], const float par_vel[3], const float par_rot[4], float time, float freq, float shape,
             float amplitude, float flat, short type, short axis, float obmat[4][4], int smooth_start)
{
	float kink[3] = {1.f, 0.f, 0.f}, par_vec[3], q1[4] = {1.f, 0.f, 0.f, 0.f};
	float t, dt = 1.f, result[3];

	if (ELEM(type, PART_KINK_NO, PART_KINK_SPIRAL))
		return;

	CLAMP(time, 0.f, 1.f);

	if (shape != 0.0f && !ELEM(type, PART_KINK_BRAID)) {
		if (shape < 0.0f)
			time = (float)pow(time, 1.f + shape);
		else
			time = (float)pow(time, 1.f / (1.f - shape));
	}

	t = time * freq * (float)M_PI;

	if (smooth_start) {
		dt = fabsf(t);
		/* smooth the beginning of kink */
		CLAMP(dt, 0.f, (float)M_PI);
		dt = sinf(dt / 2.f);
	}

	if (!ELEM(type, PART_KINK_RADIAL)) {
		float temp[3];

		kink[axis] = 1.f;

		if (obmat)
			mul_mat3_m4_v3(obmat, kink);

		mul_qt_v3(par_rot, kink);

		/* make sure kink is normal to strand */
		project_v3_v3v3(temp, kink, par_vel);
		sub_v3_v3(kink, temp);
		normalize_v3(kink);
	}

	copy_v3_v3(result, state->co);
	sub_v3_v3v3(par_vec, par_co, state->co);

	switch (type) {
		case PART_KINK_CURL:
		{
			float curl_offset[3];

			/* rotate kink vector around strand tangent */
			mul_v3_v3fl(curl_offset, kink, amplitude);
			axis_angle_to_quat(q1, par_vel, t);
			mul_qt_v3(q1, curl_offset);

			interp_v3_v3v3(par_vec, state->co, par_co, flat);
			add_v3_v3v3(result, par_vec, curl_offset);
			break;
		}
		case PART_KINK_RADIAL:
		{
			if (flat > 0.f) {
				float proj[3];
				/* flatten along strand */
				project_v3_v3v3(proj, par_vec, par_vel);
				madd_v3_v3fl(result, proj, flat);
			}

			madd_v3_v3fl(result, par_vec, -amplitude * sinf(t));
			break;
		}
		case PART_KINK_WAVE:
		{
			madd_v3_v3fl(result, kink, amplitude * sinf(t));

			if (flat > 0.f) {
				float proj[3];
				/* flatten along wave */
				project_v3_v3v3(proj, par_vec, kink);
				madd_v3_v3fl(result, proj, flat);

				/* flatten along strand */
				project_v3_v3v3(proj, par_vec, par_vel);
				madd_v3_v3fl(result, proj, flat);
			}
			break;
		}
		case PART_KINK_BRAID:
		{
			float y_vec[3] = {0.f, 1.f, 0.f};
			float z_vec[3] = {0.f, 0.f, 1.f};
			float vec_one[3], state_co[3];
			float inp_y, inp_z, length;

			if (par_rot) {
				mul_qt_v3(par_rot, y_vec);
				mul_qt_v3(par_rot, z_vec);
			}

			negate_v3(par_vec);
			normalize_v3_v3(vec_one, par_vec);

			inp_y = dot_v3v3(y_vec, vec_one);
			inp_z = dot_v3v3(z_vec, vec_one);

			if (inp_y > 0.5f) {
				copy_v3_v3(state_co, y_vec);

				mul_v3_fl(y_vec, amplitude * cosf(t));
				mul_v3_fl(z_vec, amplitude / 2.f * sinf(2.f * t));
			}
			else if (inp_z > 0.0f) {
				mul_v3_v3fl(state_co, z_vec, sinf((float)M_PI / 3.f));
				madd_v3_v3fl(state_co, y_vec, -0.5f);

				mul_v3_fl(y_vec, -amplitude * cosf(t + (float)M_PI / 3.f));
				mul_v3_fl(z_vec, amplitude / 2.f * cosf(2.f * t + (float)M_PI / 6.f));
			}
			else {
				mul_v3_v3fl(state_co, z_vec, -sinf((float)M_PI / 3.f));
				madd_v3_v3fl(state_co, y_vec, -0.5f);

				mul_v3_fl(y_vec, amplitude * -sinf(t + (float)M_PI / 6.f));
				mul_v3_fl(z_vec, amplitude / 2.f * -sinf(2.f * t + (float)M_PI / 3.f));
			}

			mul_v3_fl(state_co, amplitude);
			add_v3_v3(state_co, par_co);
			sub_v3_v3v3(par_vec, state->co, state_co);

			length = normalize_v3(par_vec);
			mul_v3_fl(par_vec, MIN2(length, amplitude / 2.f));

			add_v3_v3v3(state_co, par_co, y_vec);
			add_v3_v3(state_co, z_vec);
			add_v3_v3(state_co, par_vec);

			shape = 2.f * (float)M_PI * (1.f + shape);

			if (t < shape) {
				shape = t / shape;
				shape = (float)sqrt((double)shape);
				interp_v3_v3v3(result, result, state_co, shape);
			}
			else {
				copy_v3_v3(result, state_co);
			}
			break;
		}
	}

	/* blend the start of the kink */
	if (dt < 1.f)
		interp_v3_v3v3(state->co, state->co, result, dt);
	else
		copy_v3_v3(state->co, result);
}
Ejemplo n.º 13
0
MINLINE float normalize_v3(float n[3])
{
	return normalize_v3_v3(n, n);
}
Ejemplo n.º 14
0
int get_effector_data(EffectorCache *eff, EffectorData *efd, EffectedPoint *point, int real_velocity)
{
	float cfra = eff->scene->r.cfra;
	int ret = 0;

	if (eff->pd && eff->pd->shape==PFIELD_SHAPE_SURFACE && eff->surmd) {
		/* closest point in the object surface is an effector */
		float vec[3];

		/* using velocity corrected location allows for easier sliding over effector surface */
		copy_v3_v3(vec, point->vel);
		mul_v3_fl(vec, point->vel_to_frame);
		add_v3_v3(vec, point->loc);

		ret = closest_point_on_surface(eff->surmd, vec, efd->loc, efd->nor, real_velocity ? efd->vel : NULL);

		efd->size = 0.0f;
	}
	else if (eff->pd && eff->pd->shape==PFIELD_SHAPE_POINTS) {

		if (eff->ob->derivedFinal) {
			DerivedMesh *dm = eff->ob->derivedFinal;

			dm->getVertCo(dm, *efd->index, efd->loc);
			dm->getVertNo(dm, *efd->index, efd->nor);

			mul_m4_v3(eff->ob->obmat, efd->loc);
			mul_mat3_m4_v3(eff->ob->obmat, efd->nor);

			normalize_v3(efd->nor);

			efd->size = 0.0f;

			/**/
			ret = 1;
		}
	}
	else if (eff->psys) {
		ParticleData *pa = eff->psys->particles + *efd->index;
		ParticleKey state;

		/* exclude the particle itself for self effecting particles */
		if (eff->psys == point->psys && *efd->index == point->index) {
			/* pass */
		}
		else {
			ParticleSimulationData sim= {NULL};
			sim.scene= eff->scene;
			sim.ob= eff->ob;
			sim.psys= eff->psys;

			/* TODO: time from actual previous calculated frame (step might not be 1) */
			state.time = cfra - 1.0f;
			ret = psys_get_particle_state(&sim, *efd->index, &state, 0);

			/* TODO */
			//if (eff->pd->forcefiled == PFIELD_HARMONIC && ret==0) {
			//	if (pa->dietime < eff->psys->cfra)
			//		eff->flag |= PE_VELOCITY_TO_IMPULSE;
			//}

			copy_v3_v3(efd->loc, state.co);

			/* rather than use the velocity use rotated x-axis (defaults to velocity) */
			efd->nor[0] = 1.f;
			efd->nor[1] = efd->nor[2] = 0.f;
			mul_qt_v3(state.rot, efd->nor);
		
			if (real_velocity)
				copy_v3_v3(efd->vel, state.vel);

			efd->size = pa->size;
		}
	}
	else {
		/* use center of object for distance calculus */
		const Object *ob = eff->ob;

		/* use z-axis as normal*/
		normalize_v3_v3(efd->nor, ob->obmat[2]);

		if (eff->pd && eff->pd->shape == PFIELD_SHAPE_PLANE) {
			float temp[3], translate[3];
			sub_v3_v3v3(temp, point->loc, ob->obmat[3]);
			project_v3_v3v3(translate, temp, efd->nor);

			/* for vortex the shape chooses between old / new force */
			if (eff->pd->forcefield == PFIELD_VORTEX)
				add_v3_v3v3(efd->loc, ob->obmat[3], translate);
			else /* normally efd->loc is closest point on effector xy-plane */
				sub_v3_v3v3(efd->loc, point->loc, translate);
		}
		else {
			copy_v3_v3(efd->loc, ob->obmat[3]);
		}

		if (real_velocity)
			copy_v3_v3(efd->vel, eff->velocity);

		efd->size = 0.0f;

		ret = 1;
	}

	if (ret) {
		sub_v3_v3v3(efd->vec_to_point, point->loc, efd->loc);
		efd->distance = len_v3(efd->vec_to_point);

		/* rest length for harmonic effector, will have to see later if this could be extended to other effectors */
		if (eff->pd && eff->pd->forcefield == PFIELD_HARMONIC && eff->pd->f_size)
			mul_v3_fl(efd->vec_to_point, (efd->distance-eff->pd->f_size)/efd->distance);

		if (eff->flag & PE_USE_NORMAL_DATA) {
			copy_v3_v3(efd->vec_to_point2, efd->vec_to_point);
			copy_v3_v3(efd->nor2, efd->nor);
		}
		else {
			/* for some effectors we need the object center every time */
			sub_v3_v3v3(efd->vec_to_point2, point->loc, eff->ob->obmat[3]);
			normalize_v3_v3(efd->nor2, eff->ob->obmat[2]);
		}
	}

	return ret;
}
Ejemplo n.º 15
0
static int flyApply(bContext *C, FlyInfo *fly)
{
#define FLY_ROTATE_FAC 2.5f /* more is faster */
#define FLY_ZUP_CORRECT_FAC 0.1f /* amount to correct per step */
#define FLY_ZUP_CORRECT_ACCEL 0.05f /* increase upright momentum each step */

	/* fly mode - Shift+F
	 * a fly loop where the user can move move the view as if they are flying
	 */
	RegionView3D *rv3d = fly->rv3d;
	ARegion *ar = fly->ar;

	float mat[3][3]; /* 3x3 copy of the view matrix so we can move along the view axis */
	float dvec[3] = {0, 0, 0}; /* this is the direction thast added to the view offset per redraw */

	/* Camera Uprighting variables */
	float upvec[3] = {0, 0, 0}; /* stores the view's up vector */

	float moffset[2]; /* mouse offset from the views center */
	float tmp_quat[4]; /* used for rotating the view */

//	int cent_orig[2], /* view center */
//XXX- can avoid using //   cent[2], /* view center modified */
	int xmargin, ymargin; /* x and y margin are define the safe area where the mouses movement wont rotate the view */

#ifdef NDOF_FLY_DEBUG
	{
		static unsigned int iteration = 1;
		printf("fly timer %d\n", iteration++);
	}
#endif

	xmargin = ar->winx / 20.0f;
	ymargin = ar->winy / 20.0f;

	// UNUSED
	// cent_orig[0] = ar->winrct.xmin + ar->winx / 2;
	// cent_orig[1] = ar->winrct.ymin + ar->winy / 2;

	{

		/* mouse offset from the center */
		moffset[0] = fly->mval[0] - ar->winx / 2;
		moffset[1] = fly->mval[1] - ar->winy / 2;

		/* enforce a view margin */
		if      (moffset[0] >  xmargin) moffset[0] -= xmargin;
		else if (moffset[0] < -xmargin) moffset[0] += xmargin;
		else                            moffset[0] =  0;

		if      (moffset[1] >  ymargin) moffset[1] -= ymargin;
		else if (moffset[1] < -ymargin) moffset[1] += ymargin;
		else                            moffset[1] =  0;


		/* scale the mouse movement by this value - scales mouse movement to the view size
		 * moffset[0] / (ar->winx-xmargin * 2) - window size minus margin (same for y)
		 *
		 * the mouse moves isn't linear */

		if (moffset[0]) {
			moffset[0] /= ar->winx - (xmargin * 2);
			moffset[0] *= fabsf(moffset[0]);
		}

		if (moffset[1]) {
			moffset[1] /= ar->winy - (ymargin * 2);
			moffset[1] *= fabsf(moffset[1]);
		}

		/* Should we redraw? */
		if ((fly->speed != 0.0f) ||
		    moffset[0] || moffset[1] ||
		    (fly->zlock != FLY_AXISLOCK_STATE_OFF) ||
		    (fly->xlock != FLY_AXISLOCK_STATE_OFF) ||
		    dvec[0] || dvec[1] || dvec[2])
		{
			float dvec_tmp[3];

			/* time how fast it takes for us to redraw,
			 * this is so simple scenes don't fly too fast */
			double time_current;
			float time_redraw;
			float time_redraw_clamped;
#ifdef NDOF_FLY_DRAW_TOOMUCH
			fly->redraw = 1;
#endif
			time_current = PIL_check_seconds_timer();
			time_redraw = (float)(time_current - fly->time_lastdraw);
			time_redraw_clamped = min_ff(0.05f, time_redraw); /* clamp redraw time to avoid jitter in roll correction */
			fly->time_lastdraw = time_current;

			/* Scale the time to use shift to scale the speed down- just like
			 * shift slows many other areas of blender down */
			if (fly->use_precision)
				fly->speed = fly->speed * (1.0f - time_redraw_clamped);

			copy_m3_m4(mat, rv3d->viewinv);

			if (fly->pan_view == true) {
				/* pan only */
				dvec_tmp[0] = -moffset[0];
				dvec_tmp[1] = -moffset[1];
				dvec_tmp[2] = 0;

				if (fly->use_precision) {
					dvec_tmp[0] *= 0.1f;
					dvec_tmp[1] *= 0.1f;
				}

				mul_m3_v3(mat, dvec_tmp);
				mul_v3_fl(dvec_tmp, time_redraw * 200.0f * fly->grid);
			}
			else {
				float roll; /* similar to the angle between the camera's up and the Z-up,
				             * but its very rough so just roll */

				/* rotate about the X axis- look up/down */
				if (moffset[1]) {
					upvec[0] = 1;
					upvec[1] = 0;
					upvec[2] = 0;
					mul_m3_v3(mat, upvec);
					/* Rotate about the relative up vec */
					axis_angle_to_quat(tmp_quat, upvec, (float)moffset[1] * time_redraw * -FLY_ROTATE_FAC);
					mul_qt_qtqt(rv3d->viewquat, rv3d->viewquat, tmp_quat);

					if (fly->xlock != FLY_AXISLOCK_STATE_OFF)
						fly->xlock = FLY_AXISLOCK_STATE_ACTIVE;  /* check for rotation */
					if (fly->zlock != FLY_AXISLOCK_STATE_OFF)
						fly->zlock = FLY_AXISLOCK_STATE_ACTIVE;
					fly->xlock_momentum = 0.0f;
				}

				/* rotate about the Y axis- look left/right */
				if (moffset[0]) {

					/* if we're upside down invert the moffset */
					upvec[0] = 0.0f;
					upvec[1] = 1.0f;
					upvec[2] = 0.0f;
					mul_m3_v3(mat, upvec);

					if (upvec[2] < 0.0f)
						moffset[0] = -moffset[0];

					/* make the lock vectors */
					if (fly->zlock) {
						upvec[0] = 0.0f;
						upvec[1] = 0.0f;
						upvec[2] = 1.0f;
					}
					else {
						upvec[0] = 0.0f;
						upvec[1] = 1.0f;
						upvec[2] = 0.0f;
						mul_m3_v3(mat, upvec);
					}

					/* Rotate about the relative up vec */
					axis_angle_to_quat(tmp_quat, upvec, (float)moffset[0] * time_redraw * FLY_ROTATE_FAC);
					mul_qt_qtqt(rv3d->viewquat, rv3d->viewquat, tmp_quat);

					if (fly->xlock != FLY_AXISLOCK_STATE_OFF)
						fly->xlock = FLY_AXISLOCK_STATE_ACTIVE;  /* check for rotation */
					if (fly->zlock != FLY_AXISLOCK_STATE_OFF)
						fly->zlock = FLY_AXISLOCK_STATE_ACTIVE;
				}

				if (fly->zlock == FLY_AXISLOCK_STATE_ACTIVE) {
					upvec[0] = 1.0f;
					upvec[1] = 0.0f;
					upvec[2] = 0.0f;
					mul_m3_v3(mat, upvec);

					/* make sure we have some z rolling */
					if (fabsf(upvec[2]) > 0.00001f) {
						roll = upvec[2] * 5.0f;
						upvec[0] = 0.0f; /* rotate the view about this axis */
						upvec[1] = 0.0f;
						upvec[2] = 1.0f;

						mul_m3_v3(mat, upvec);
						/* Rotate about the relative up vec */
						axis_angle_to_quat(tmp_quat, upvec,
						                   roll * time_redraw_clamped * fly->zlock_momentum * FLY_ZUP_CORRECT_FAC);
						mul_qt_qtqt(rv3d->viewquat, rv3d->viewquat, tmp_quat);

						fly->zlock_momentum += FLY_ZUP_CORRECT_ACCEL;
					}
					else {
						fly->zlock = FLY_AXISLOCK_STATE_IDLE; /* don't check until the view rotates again */
						fly->zlock_momentum = 0.0f;
					}
				}

				/* only apply xcorrect when mouse isn't applying x rot */
				if (fly->xlock == FLY_AXISLOCK_STATE_ACTIVE && moffset[1] == 0) {
					upvec[0] = 0;
					upvec[1] = 0;
					upvec[2] = 1;
					mul_m3_v3(mat, upvec);
					/* make sure we have some z rolling */
					if (fabsf(upvec[2]) > 0.00001f) {
						roll = upvec[2] * -5.0f;

						upvec[0] = 1.0f; /* rotate the view about this axis */
						upvec[1] = 0.0f;
						upvec[2] = 0.0f;

						mul_m3_v3(mat, upvec);

						/* Rotate about the relative up vec */
						axis_angle_to_quat(tmp_quat, upvec, roll * time_redraw_clamped * fly->xlock_momentum * 0.1f);
						mul_qt_qtqt(rv3d->viewquat, rv3d->viewquat, tmp_quat);

						fly->xlock_momentum += 0.05f;
					}
					else {
						fly->xlock = FLY_AXISLOCK_STATE_IDLE; /* see above */
						fly->xlock_momentum = 0.0f;
					}
				}

				if (fly->axis == -1) {
					/* pause */
					zero_v3(dvec_tmp);
				}
				else if (!fly->use_freelook) {
					/* Normal operation */
					/* define dvec, view direction vector */
					zero_v3(dvec_tmp);
					/* move along the current axis */
					dvec_tmp[fly->axis] = 1.0f;

					mul_m3_v3(mat, dvec_tmp);
				}
				else {
					normalize_v3_v3(dvec_tmp, fly->dvec_prev);
					if (fly->speed < 0.0f) {
						negate_v3(dvec_tmp);
					}
				}

				mul_v3_fl(dvec_tmp, fly->speed * time_redraw * 0.25f);
			}

			/* impose a directional lag */
			interp_v3_v3v3(dvec, dvec_tmp, fly->dvec_prev, (1.0f / (1.0f + (time_redraw * 5.0f))));

			if (rv3d->persp == RV3D_CAMOB) {
				Object *lock_ob = fly->root_parent ? fly->root_parent : fly->v3d->camera;
				if (lock_ob->protectflag & OB_LOCK_LOCX) dvec[0] = 0.0;
				if (lock_ob->protectflag & OB_LOCK_LOCY) dvec[1] = 0.0;
				if (lock_ob->protectflag & OB_LOCK_LOCZ) dvec[2] = 0.0;
			}

			add_v3_v3(rv3d->ofs, dvec);

			if (rv3d->persp == RV3D_CAMOB) {
				const bool do_rotate = ((fly->xlock != FLY_AXISLOCK_STATE_OFF) ||
				                        (fly->zlock != FLY_AXISLOCK_STATE_OFF) ||
				                        ((moffset[0] || moffset[1]) && !fly->pan_view));
				const bool do_translate = (fly->speed != 0.0f || fly->pan_view);
				flyMoveCamera(C, rv3d, fly, do_rotate, do_translate);
			}

		}
		else {
			/* we're not redrawing but we need to update the time else the view will jump */
			fly->time_lastdraw = PIL_check_seconds_timer();
		}
		/* end drawing */
		copy_v3_v3(fly->dvec_prev, dvec);
	}

	return OPERATOR_FINISHED;
}
Ejemplo n.º 16
0
static DerivedMesh *applyModifier(ModifierData *md, Object *ob,
                                  DerivedMesh *derivedData,
                                  int useRenderParams,
                                  int UNUSED(isFinalCalc))
{
    DerivedMesh *dm= derivedData;
    DerivedMesh *result;
    ScrewModifierData *ltmd= (ScrewModifierData*) md;

    int *origindex;
    int mface_index=0;
    int step;
    int i, j;
    int i1,i2;
    int step_tot= useRenderParams ? ltmd->render_steps : ltmd->steps;
    const int do_flip = ltmd->flag & MOD_SCREW_NORMAL_FLIP ? 1 : 0;
    int maxVerts=0, maxEdges=0, maxFaces=0;
    int totvert= dm->getNumVerts(dm);
    int totedge= dm->getNumEdges(dm);

    char axis_char= 'X', close;
    float angle= ltmd->angle;
    float screw_ofs= ltmd->screw_ofs;
    float axis_vec[3]= {0.0f, 0.0f, 0.0f};
    float tmp_vec1[3], tmp_vec2[3];
    float mat3[3][3];
    float mtx_tx[4][4]; /* transform the coords by an object relative to this objects transformation */
    float mtx_tx_inv[4][4]; /* inverted */
    float mtx_tmp_a[4][4];

    int vc_tot_linked= 0;
    short other_axis_1, other_axis_2;
    float *tmpf1, *tmpf2;

    MFace *mface_new, *mf_new;
    MEdge *medge_orig, *med_orig, *med_new, *med_new_firstloop, *medge_new;
    MVert *mvert_new, *mvert_orig, *mv_orig, *mv_new, *mv_new_base;

    ScrewVertConnect *vc, *vc_tmp, *vert_connect= NULL;

    /* dont do anything? */
    if (!totvert)
        return CDDM_from_template(dm, 0, 0, 0);

    switch(ltmd->axis) {
    case 0:
        other_axis_1=1;
        other_axis_2=2;
        break;
    case 1:
        other_axis_1=0;
        other_axis_2=2;
        break;
    default: /* 2, use default to quiet warnings */
        other_axis_1=0;
        other_axis_2=1;
        break;
    }

    axis_vec[ltmd->axis]= 1.0f;

    if (ltmd->ob_axis) {
        /* calc the matrix relative to the axis object */
        invert_m4_m4(mtx_tmp_a, ob->obmat);
        copy_m4_m4(mtx_tx_inv, ltmd->ob_axis->obmat);
        mul_m4_m4m4(mtx_tx, mtx_tx_inv, mtx_tmp_a);

        /* calc the axis vec */
        mul_mat3_m4_v3(mtx_tx, axis_vec); /* only rotation component */
        normalize_v3(axis_vec);

        /* screw */
        if(ltmd->flag & MOD_SCREW_OBJECT_OFFSET) {
            /* find the offset along this axis relative to this objects matrix */
            float totlen = len_v3(mtx_tx[3]);

            if(totlen != 0.0f) {
                float zero[3]= {0.0f, 0.0f, 0.0f};
                float cp[3];
                screw_ofs= closest_to_line_v3(cp, mtx_tx[3], zero, axis_vec);
            }
            else {
                screw_ofs= 0.0f;
            }
        }

        /* angle */

#if 0	// cant incluide this, not predictable enough, though quite fun,.
        if(ltmd->flag & MOD_SCREW_OBJECT_ANGLE) {
            float mtx3_tx[3][3];
            copy_m3_m4(mtx3_tx, mtx_tx);

            float vec[3] = {0,1,0};
            float cross1[3];
            float cross2[3];
            cross_v3_v3v3(cross1, vec, axis_vec);

            mul_v3_m3v3(cross2, mtx3_tx, cross1);
            {
                float c1[3];
                float c2[3];
                float axis_tmp[3];

                cross_v3_v3v3(c1, cross2, axis_vec);
                cross_v3_v3v3(c2, axis_vec, c1);


                angle= angle_v3v3(cross1, c2);

                cross_v3_v3v3(axis_tmp, cross1, c2);
                normalize_v3(axis_tmp);

                if(len_v3v3(axis_tmp, axis_vec) > 1.0f)
                    angle= -angle;

            }
        }
#endif
    }
    else {
        /* exis char is used by i_rotate*/
        axis_char += ltmd->axis; /* 'X' + axis */

        /* useful to be able to use the axis vec in some cases still */
        zero_v3(axis_vec);
        axis_vec[ltmd->axis]= 1.0f;
    }

    /* apply the multiplier */
    angle *= ltmd->iter;
    screw_ofs *= ltmd->iter;

    /* multiplying the steps is a bit tricky, this works best */
    step_tot = ((step_tot + 1) * ltmd->iter) - (ltmd->iter - 1);

    /* will the screw be closed?
     * Note! smaller then FLT_EPSILON*100 gives problems with float precision so its never closed. */
    if (fabsf(screw_ofs) <= (FLT_EPSILON*100.0f) && fabsf(fabsf(angle) - ((float)M_PI * 2.0f)) <= (FLT_EPSILON*100.0f)) {
        close= 1;
        step_tot--;
        if(step_tot < 3) step_tot= 3;

        maxVerts =	totvert  * step_tot; /* -1 because we're joining back up */
        maxEdges =	(totvert * step_tot) + /* these are the edges between new verts */
                    (totedge * step_tot); /* -1 because vert edges join */
        maxFaces =	totedge * step_tot;

        screw_ofs= 0.0f;
    }
    else {
        close= 0;
        if(step_tot < 3) step_tot= 3;

        maxVerts =	totvert  * step_tot; /* -1 because we're joining back up */
        maxEdges =	(totvert * (step_tot-1)) + /* these are the edges between new verts */
                    (totedge * step_tot); /* -1 because vert edges join */
        maxFaces =	totedge * (step_tot-1);
    }

    result= CDDM_from_template(dm, maxVerts, maxEdges, maxFaces);

    /* copy verts from mesh */
    mvert_orig =	dm->getVertArray(dm);
    medge_orig =	dm->getEdgeArray(dm);

    mvert_new =		result->getVertArray(result);
    mface_new =		result->getFaceArray(result);
    medge_new =		result->getEdgeArray(result);

    origindex= result->getFaceDataArray(result, CD_ORIGINDEX);

    DM_copy_vert_data(dm, result, 0, 0, totvert); /* copy first otherwise this overwrites our own vertex normals */

    /* Set the locations of the first set of verts */

    mv_new= mvert_new;
    mv_orig= mvert_orig;

    /* Copy the first set of edges */
    med_orig= medge_orig;
    med_new= medge_new;
    for (i=0; i < totedge; i++, med_orig++, med_new++) {
        med_new->v1= med_orig->v1;
        med_new->v2= med_orig->v2;
        med_new->crease= med_orig->crease;
        med_new->flag= med_orig->flag &  ~ME_LOOSEEDGE;
    }

    if(ltmd->flag & MOD_SCREW_NORMAL_CALC) {
        /*
         * Normal Calculation (for face flipping)
         * Sort edge verts for correct face flipping
         * NOT REALLY NEEDED but face flipping is nice.
         *
         * */


        /* Notice!
         *
         * Since we are only ordering the edges here it can avoid mallocing the
         * extra space by abusing the vert array berfore its filled with new verts.
         * The new array for vert_connect must be at least sizeof(ScrewVertConnect) * totvert
         * and the size of our resulting meshes array is sizeof(MVert) * totvert * 3
         * so its safe to use the second 2 thrids of MVert the array for vert_connect,
         * just make sure ScrewVertConnect struct is no more then twice as big as MVert,
         * at the moment there is no chance of that being a problem,
         * unless MVert becomes half its current size.
         *
         * once the edges are ordered, vert_connect is not needed and it can be used for verts
         *
         * This makes the modifier faster with one less alloc.
         */

        vert_connect= MEM_mallocN(sizeof(ScrewVertConnect) * totvert, "ScrewVertConnect");
        //vert_connect= (ScrewVertConnect *) &medge_new[totvert]; /* skip the first slice of verts */
        vc= vert_connect;

        /* Copy Vert Locations */
        /* - We can do this in a later loop - only do here if no normal calc */
        if (!totedge) {
            for (i=0; i < totvert; i++, mv_orig++, mv_new++) {
                copy_v3_v3(mv_new->co, mv_orig->co);
                normalize_v3_v3(vc->no, mv_new->co); /* no edges- this is really a dummy normal */
            }
        }
        else {
            /*printf("\n\n\n\n\nStarting Modifier\n");*/
            /* set edge users */
            med_new= medge_new;
            mv_new= mvert_new;

            if (ltmd->ob_axis) {
                /*mtx_tx is initialized early on */
                for (i=0; i < totvert; i++, mv_new++, mv_orig++, vc++) {
                    vc->co[0]= mv_new->co[0]= mv_orig->co[0];
                    vc->co[1]= mv_new->co[1]= mv_orig->co[1];
                    vc->co[2]= mv_new->co[2]= mv_orig->co[2];

                    vc->flag= 0;
                    vc->e[0]= vc->e[1]= NULL;
                    vc->v[0]= vc->v[1]= -1;

                    mul_m4_v3(mtx_tx, vc->co);
                    /* length in 2d, dont sqrt because this is only for comparison */
                    vc->dist =	vc->co[other_axis_1]*vc->co[other_axis_1] +
                                vc->co[other_axis_2]*vc->co[other_axis_2];

                    /* printf("location %f %f %f -- %f\n", vc->co[0], vc->co[1], vc->co[2], vc->dist);*/
                }
            }
            else {
                for (i=0; i < totvert; i++, mv_new++, mv_orig++, vc++) {
                    vc->co[0]= mv_new->co[0]= mv_orig->co[0];
                    vc->co[1]= mv_new->co[1]= mv_orig->co[1];
                    vc->co[2]= mv_new->co[2]= mv_orig->co[2];

                    vc->flag= 0;
                    vc->e[0]= vc->e[1]= NULL;
                    vc->v[0]= vc->v[1]= -1;

                    /* length in 2d, dont sqrt because this is only for comparison */
                    vc->dist =	vc->co[other_axis_1]*vc->co[other_axis_1] +
                                vc->co[other_axis_2]*vc->co[other_axis_2];

                    /* printf("location %f %f %f -- %f\n", vc->co[0], vc->co[1], vc->co[2], vc->dist);*/
                }
            }

            /* this loop builds connectivity info for verts */
            for (i=0; i<totedge; i++, med_new++) {
                vc= &vert_connect[med_new->v1];

                if (vc->v[0] == -1) { /* unused */
                    vc->v[0]= med_new->v2;
                    vc->e[0]= med_new;
                }
                else if (vc->v[1] == -1) {
                    vc->v[1]= med_new->v2;
                    vc->e[1]= med_new;
                }
                else {
                    vc->v[0]= vc->v[1]= -2; /* erro value  - dont use, 3 edges on vert */
                }

                vc= &vert_connect[med_new->v2];

                /* same as above but swap v1/2 */
                if (vc->v[0] == -1) { /* unused */
                    vc->v[0]= med_new->v1;
                    vc->e[0]= med_new;
                }
                else if (vc->v[1] == -1) {
                    vc->v[1]= med_new->v1;
                    vc->e[1]= med_new;
                }
                else {
                    vc->v[0]= vc->v[1]= -2; /* erro value  - dont use, 3 edges on vert */
                }
            }

            /* find the first vert */
            vc= vert_connect;
            for (i=0; i < totvert; i++, vc++) {
                /* Now do search for connected verts, order all edges and flip them
                 * so resulting faces are flipped the right way */
                vc_tot_linked= 0; /* count the number of linked verts for this loop */
                if (vc->flag == 0) {
                    int v_best=-1, ed_loop_closed=0; /* vert and vert new */
                    ScrewVertIter lt_iter;
                    int ed_loop_flip= 0; /* compiler complains if not initialized, but it should be initialized below */
                    float fl= -1.0f;

                    /*printf("Loop on connected vert: %i\n", i);*/

                    for(j=0; j<2; j++) {
                        /*printf("\tSide: %i\n", j);*/
                        screwvert_iter_init(&lt_iter, vert_connect, i, j);
                        if (j == 1) {
                            screwvert_iter_step(&lt_iter);
                        }
                        while (lt_iter.v_poin) {
                            /*printf("\t\tVERT: %i\n", lt_iter.v);*/
                            if (lt_iter.v_poin->flag) {
                                /*printf("\t\t\tBreaking Found end\n");*/
                                //endpoints[0]= endpoints[1]= -1;
                                ed_loop_closed= 1; /* circle */
                                break;
                            }
                            lt_iter.v_poin->flag= 1;
                            vc_tot_linked++;
                            /*printf("Testing 2 floats %f : %f\n", fl, lt_iter.v_poin->dist);*/
                            if (fl <= lt_iter.v_poin->dist) {
                                fl= lt_iter.v_poin->dist;
                                v_best= lt_iter.v;
                                /*printf("\t\t\tVERT BEST: %i\n", v_best);*/
                            }
                            screwvert_iter_step(&lt_iter);
                            if (!lt_iter.v_poin) {
                                /*printf("\t\t\tFound End Also Num %i\n", j);*/
                                /*endpoints[j]= lt_iter.v_other;*/ /* other is still valid */
                                break;
                            }
                        }
                    }

                    /* now we have a collection of used edges. flip their edges the right way*/
                    /*if (v_best != -1) - */

                    /*printf("Done Looking - vc_tot_linked: %i\n", vc_tot_linked);*/

                    if (vc_tot_linked>1) {
                        float vf_1, vf_2, vf_best;

                        vc_tmp= &vert_connect[v_best];

                        tmpf1= vert_connect[vc_tmp->v[0]].co;
                        tmpf2= vert_connect[vc_tmp->v[1]].co;


                        /* edge connects on each side! */
                        if ((vc_tmp->v[0] > -1) && (vc_tmp->v[1] > -1)) {
                            /*printf("Verts on each side (%i %i)\n", vc_tmp->v[0], vc_tmp->v[1]);*/
                            /* find out which is higher */

                            vf_1= tmpf1[ltmd->axis];
                            vf_2= tmpf2[ltmd->axis];
                            vf_best= vc_tmp->co[ltmd->axis];

                            if (vf_1 < vf_best && vf_best < vf_2) {
                                ed_loop_flip= 0;
                            }
                            else if (vf_1 > vf_best && vf_best > vf_2) {
                                ed_loop_flip= 1;
                            }
                            else {
                                /* not so simple to work out which edge is higher */
                                sub_v3_v3v3(tmp_vec1, tmpf1, vc_tmp->co);
                                sub_v3_v3v3(tmp_vec2, tmpf2, vc_tmp->co);
                                normalize_v3(tmp_vec1);
                                normalize_v3(tmp_vec2);

                                if (tmp_vec1[ltmd->axis] < tmp_vec2[ltmd->axis]) {
                                    ed_loop_flip= 1;
                                }
                                else {
                                    ed_loop_flip= 0;
                                }
                            }
                        }
                        else if (vc_tmp->v[0] >= 0) { /*vertex only connected on 1 side */
                            /*printf("Verts on ONE side (%i %i)\n", vc_tmp->v[0], vc_tmp->v[1]);*/
                            if (tmpf1[ltmd->axis] < vc_tmp->co[ltmd->axis]) { /* best is above */
                                ed_loop_flip= 1;
                            }
                            else { /* best is below or even... in even case we cant know whet  to do. */
                                ed_loop_flip= 0;
                            }

                        }/* else {
							printf("No Connected ___\n");
						}*/

                        /*printf("flip direction %i\n", ed_loop_flip);*/


                        /* switch the flip option if set
                         * note: flip is now done at face level so copying vgroup slizes is easier */
                        /*
                        if (do_flip)
                        	ed_loop_flip= !ed_loop_flip;
                        */

                        if (angle < 0.0f)
                            ed_loop_flip= !ed_loop_flip;

                        /* if its closed, we only need 1 loop */
                        for(j=ed_loop_closed; j<2; j++) {
                            /*printf("Ordering Side J %i\n", j);*/

                            screwvert_iter_init(&lt_iter, vert_connect, v_best, j);
                            /*printf("\n\nStarting - Loop\n");*/
                            lt_iter.v_poin->flag= 1; /* so a non loop will traverse the other side */


                            /* If this is the vert off the best vert and
                             * the best vert has 2 edges connected too it
                             * then swap the flip direction */
                            if (j == 1 && (vc_tmp->v[0] > -1) && (vc_tmp->v[1] > -1))
                                ed_loop_flip= !ed_loop_flip;

                            while (lt_iter.v_poin && lt_iter.v_poin->flag != 2) {
                                /*printf("\tOrdering Vert V %i\n", lt_iter.v);*/

                                lt_iter.v_poin->flag= 2;
                                if (lt_iter.e) {
                                    if (lt_iter.v == lt_iter.e->v1) {
                                        if (ed_loop_flip == 0) {
                                            /*printf("\t\t\tFlipping 0\n");*/
                                            SWAP(int, lt_iter.e->v1, lt_iter.e->v2);
                                        }/* else {
											printf("\t\t\tFlipping Not 0\n");
										}*/
                                    }
                                    else if (lt_iter.v == lt_iter.e->v2) {
                                        if (ed_loop_flip == 1) {
                                            /*printf("\t\t\tFlipping 1\n");*/
                                            SWAP(int, lt_iter.e->v1, lt_iter.e->v2);
                                        }/* else {
											printf("\t\t\tFlipping Not 1\n");
										}*/
                                    }/* else {
										printf("\t\tIncorrect edge topology");
									}*/
                                }/* else {
									printf("\t\tNo Edge at this point\n");
								}*/
                                screwvert_iter_step(&lt_iter);
                            }
                        }
Ejemplo n.º 17
0
static Object *boid_find_ground(BoidBrainData *bbd, ParticleData *pa, float ground_co[3], float ground_nor[3])
{
	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 = len_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(col.md->bvhtree, col.co1, ray_dir, radius, &hit, BKE_psys_collision_neartest_cb, &col);
		}
		/* 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 = len_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(col.md->bvhtree, col.co1, ray_dir, radius, &hit, BKE_psys_collision_neartest_cb, &col);
		}
		/* 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;
	}
}
Ejemplo n.º 18
0
static int rule_fight(BoidRule *rule, BoidBrainData *bbd, BoidValues *val, ParticleData *pa)
{
	BoidRuleFight *fbr = (BoidRuleFight*)rule;
	KDTreeNearest *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_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_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;
}
Ejemplo n.º 19
0
float normalize_v3(float r[3])
{
	return normalize_v3_v3(r, r);
}
Ejemplo n.º 20
0
static int mesh_bisect_exec(bContext *C, wmOperator *op)
{
	Scene *scene = CTX_data_scene(C);

	/* both can be NULL, fallbacks values are used */
	View3D *v3d = CTX_wm_view3d(C);
	RegionView3D *rv3d = ED_view3d_context_rv3d(C);

	Object *obedit = CTX_data_edit_object(C);
	BMEditMesh *em = BKE_editmesh_from_object(obedit);
	BMesh *bm;
	BMOperator bmop;
	float plane_co[3];
	float plane_no[3];
	float imat[4][4];

	const float thresh = RNA_float_get(op->ptr, "threshold");
	const bool use_fill = RNA_boolean_get(op->ptr, "use_fill");
	const bool clear_inner = RNA_boolean_get(op->ptr, "clear_inner");
	const bool clear_outer = RNA_boolean_get(op->ptr, "clear_outer");

	PropertyRNA *prop_plane_co;
	PropertyRNA *prop_plane_no;

	prop_plane_co = RNA_struct_find_property(op->ptr, "plane_co");
	if (RNA_property_is_set(op->ptr, prop_plane_co)) {
		RNA_property_float_get_array(op->ptr, prop_plane_co, plane_co);
	}
	else {
		copy_v3_v3(plane_co, ED_view3d_cursor3d_get(scene, v3d));
		RNA_property_float_set_array(op->ptr, prop_plane_co, plane_co);
	}

	prop_plane_no = RNA_struct_find_property(op->ptr, "plane_no");
	if (RNA_property_is_set(op->ptr, prop_plane_no)) {
		RNA_property_float_get_array(op->ptr, prop_plane_no, plane_no);
	}
	else {
		if (rv3d) {
			copy_v3_v3(plane_no, rv3d->viewinv[1]);
		}
		else {
			/* fallback... */
			plane_no[0] = plane_no[1] = 0.0f; plane_no[2] = 1.0f;
		}
		RNA_property_float_set_array(op->ptr, prop_plane_no, plane_no);
	}



	/* -------------------------------------------------------------------- */
	/* Modal support */
	/* Note: keep this isolated, exec can work wihout this */
	if ((op->customdata != NULL) &&
	    mesh_bisect_interactive_calc(C, op, em, plane_co, plane_no))
	{
		/* write back to the props */
		RNA_property_float_set_array(op->ptr, prop_plane_no, plane_no);
		RNA_property_float_set_array(op->ptr, prop_plane_co, plane_co);
	}
	/* End Modal */
	/* -------------------------------------------------------------------- */



	bm = em->bm;

	invert_m4_m4(imat, obedit->obmat);
	mul_m4_v3(imat, plane_co);
	mul_mat3_m4_v3(imat, plane_no);

	EDBM_op_init(em, &bmop, op,
	             "bisect_plane geom=%hvef plane_co=%v plane_no=%v dist=%f clear_inner=%b clear_outer=%b",
	             BM_ELEM_SELECT, plane_co, plane_no, thresh, clear_inner, clear_outer);
	BMO_op_exec(bm, &bmop);

	EDBM_flag_disable_all(em, BM_ELEM_SELECT);

	if (use_fill) {
		float normal_fill[3];
		BMOperator bmop_fill;
		BMOperator bmop_attr;

		normalize_v3_v3(normal_fill, plane_no);
		if (clear_outer == true && clear_inner == false) {
			negate_v3(normal_fill);
		}

		/* Fill */
		BMO_op_initf(
		        bm, &bmop_fill, op->flag,
		        "triangle_fill edges=%S normal=%v use_dissolve=%b",
		        &bmop, "geom_cut.out", normal_fill, true);
		BMO_op_exec(bm, &bmop_fill);

		/* Copy Attributes */
		BMO_op_initf(bm, &bmop_attr, op->flag,
		             "face_attribute_fill faces=%S use_normals=%b use_data=%b",
		             &bmop_fill, "geom.out", false, true);
		BMO_op_exec(bm, &bmop_attr);

		BMO_slot_buffer_hflag_enable(bm, bmop_fill.slots_out, "geom.out", BM_FACE, BM_ELEM_SELECT, true);

		BMO_op_finish(bm, &bmop_attr);
		BMO_op_finish(bm, &bmop_fill);
	}

	BMO_slot_buffer_hflag_enable(bm, bmop.slots_out, "geom_cut.out", BM_VERT | BM_EDGE, BM_ELEM_SELECT, true);

	if (!EDBM_op_finish(em, &bmop, op, true)) {
		return OPERATOR_CANCELLED;
	}
	else {
		EDBM_update_generic(em, true, true);
		EDBM_selectmode_flush(em);
		return OPERATOR_FINISHED;
	}
}
Ejemplo n.º 21
0
/* ***** actual texture sampling ***** */
int ocean_texture(Tex *tex, const float texvec[2], TexResult *texres)
{
	OceanTex *ot = tex->ot;
	ModifierData *md;
	OceanModifierData *omd;

	texres->tin = 0.0f;

	if ( !(ot) ||
	     !(ot->object) ||
	     !(md  = (ModifierData *)modifiers_findByType(ot->object, eModifierType_Ocean)) ||
	     !(omd = (OceanModifierData *)md)->ocean)
	{
		return 0;
	}
	else {
		const int do_normals = (omd->flag & MOD_OCEAN_GENERATE_NORMALS);
		int cfra = R.r.cfra;
		int retval = TEX_INT;

		OceanResult ocr;
		const float u = 0.5f + 0.5f * texvec[0];
		const float v = 0.5f + 0.5f * texvec[1];

		if (omd->oceancache && omd->cached == true) {

			CLAMP(cfra, omd->bakestart, omd->bakeend);
			cfra -= omd->bakestart;	/* shift to 0 based */

			BKE_ocean_cache_eval_uv(omd->oceancache, &ocr, cfra, u, v);

		}
		else {	/* non-cached */

			if (G.is_rendering)
				BKE_ocean_eval_uv_catrom(omd->ocean, &ocr, u, v);
			else
				BKE_ocean_eval_uv(omd->ocean, &ocr, u, v);

			ocr.foam = BKE_ocean_jminus_to_foam(ocr.Jminus, omd->foam_coverage);
		}

		switch (ot->output) {
			case TEX_OCN_DISPLACEMENT:
				/* XYZ displacement */
				texres->tr = 0.5f + 0.5f * ocr.disp[0];
				texres->tg = 0.5f + 0.5f * ocr.disp[2];
				texres->tb = 0.5f + 0.5f * ocr.disp[1];

				texres->tr = MAX2(0.0f, texres->tr);
				texres->tg = MAX2(0.0f, texres->tg);
				texres->tb = MAX2(0.0f, texres->tb);

				BRICONTRGB;

				retval = TEX_RGB;
				break;

			case TEX_OCN_EMINUS:
				/* -ve eigenvectors ? */
				texres->tr = ocr.Eminus[0];
				texres->tg = ocr.Eminus[2];
				texres->tb = ocr.Eminus[1];
				retval = TEX_RGB;
				break;

			case TEX_OCN_EPLUS:
				/* -ve eigenvectors ? */
				texres->tr = ocr.Eplus[0];
				texres->tg = ocr.Eplus[2];
				texres->tb = ocr.Eplus[1];
				retval = TEX_RGB;
				break;

			case TEX_OCN_JPLUS:
				texres->tin = ocr.Jplus;
				retval = TEX_INT;
				break;

			case TEX_OCN_FOAM:

				texres->tin = ocr.foam;

				BRICONT;

				retval = TEX_INT;
				break;
		}

		/* if normals needed */

		if (texres->nor && do_normals) {
			normalize_v3_v3(texres->nor, ocr.normal);
			retval |= TEX_NOR;
		}

		texres->ta = 1.0f;

		return retval;
	}
}
Ejemplo n.º 22
0
static DerivedMesh *applyModifier(
        ModifierData *md, Object *ob,
        DerivedMesh *dm,
        ModifierApplyFlag UNUSED(flag))
{
	DerivedMesh *result;
	const SolidifyModifierData *smd = (SolidifyModifierData *) md;

	MVert *mv, *mvert, *orig_mvert;
	MEdge *ed, *medge, *orig_medge;
	MLoop *ml, *mloop, *orig_mloop;
	MPoly *mp, *mpoly, *orig_mpoly;
	const unsigned int numVerts = (unsigned int)dm->getNumVerts(dm);
	const unsigned int numEdges = (unsigned int)dm->getNumEdges(dm);
	const unsigned int numFaces = (unsigned int)dm->getNumPolys(dm);
	const unsigned int numLoops = (unsigned int)dm->getNumLoops(dm);
	unsigned int newLoops = 0, newFaces = 0, newEdges = 0, newVerts = 0, rimVerts = 0;

	/* only use material offsets if we have 2 or more materials  */
	const short mat_nr_max = ob->totcol > 1 ? ob->totcol - 1 : 0;
	const short mat_ofs = mat_nr_max ? smd->mat_ofs : 0;
	const short mat_ofs_rim = mat_nr_max ? smd->mat_ofs_rim : 0;

	/* use for edges */
	/* over-alloc new_vert_arr, old_vert_arr */
	unsigned int *new_vert_arr = NULL;
	STACK_DECLARE(new_vert_arr);

	unsigned int *new_edge_arr = NULL;
	STACK_DECLARE(new_edge_arr);

	unsigned int *old_vert_arr = MEM_callocN(sizeof(*old_vert_arr) * (size_t)numVerts, "old_vert_arr in solidify");

	unsigned int *edge_users = NULL;
	char *edge_order = NULL;

	float (*vert_nors)[3] = NULL;
	float (*face_nors)[3] = NULL;

	const bool need_face_normals = (smd->flag & MOD_SOLIDIFY_NORMAL_CALC) || (smd->flag & MOD_SOLIDIFY_EVEN);

	const float ofs_orig = -(((-smd->offset_fac + 1.0f) * 0.5f) * smd->offset);
	const float ofs_new  = smd->offset + ofs_orig;
	const float offset_fac_vg = smd->offset_fac_vg;
	const float offset_fac_vg_inv = 1.0f - smd->offset_fac_vg;
	const bool do_flip = (smd->flag & MOD_SOLIDIFY_FLIP) != 0;
	const bool do_clamp = (smd->offset_clamp != 0.0f);
	const bool do_shell = ((smd->flag & MOD_SOLIDIFY_RIM) && (smd->flag & MOD_SOLIDIFY_NOSHELL)) == 0;

	/* weights */
	MDeformVert *dvert;
	const bool defgrp_invert = (smd->flag & MOD_SOLIDIFY_VGROUP_INV) != 0;
	int defgrp_index;

	/* array size is doubled in case of using a shell */
	const unsigned int stride = do_shell ? 2 : 1;

	modifier_get_vgroup(ob, dm, smd->defgrp_name, &dvert, &defgrp_index);

	orig_mvert = dm->getVertArray(dm);
	orig_medge = dm->getEdgeArray(dm);
	orig_mloop = dm->getLoopArray(dm);
	orig_mpoly = dm->getPolyArray(dm);

	if (need_face_normals) {
		/* calculate only face normals */
		face_nors = MEM_mallocN(sizeof(*face_nors) * (size_t)numFaces, __func__);
		BKE_mesh_calc_normals_poly(
		            orig_mvert, NULL, (int)numVerts,
		            orig_mloop, orig_mpoly,
		            (int)numLoops, (int)numFaces,
		            face_nors, true);
	}

	STACK_INIT(new_vert_arr, numVerts * 2);
	STACK_INIT(new_edge_arr, numEdges * 2);

	if (smd->flag & MOD_SOLIDIFY_RIM) {
		BLI_bitmap *orig_mvert_tag = BLI_BITMAP_NEW(numVerts, __func__);
		unsigned int eidx;
		unsigned int i;

#define INVALID_UNUSED ((unsigned int)-1)
#define INVALID_PAIR ((unsigned int)-2)

		new_vert_arr = MEM_mallocN(sizeof(*new_vert_arr) * (size_t)(numVerts * 2), __func__);
		new_edge_arr = MEM_mallocN(sizeof(*new_edge_arr) * (size_t)((numEdges * 2) + numVerts), __func__);

		edge_users = MEM_mallocN(sizeof(*edge_users) * (size_t)numEdges, "solid_mod edges");
		edge_order = MEM_mallocN(sizeof(*edge_order) * (size_t)numEdges, "solid_mod eorder");


		/* save doing 2 loops here... */
#if 0
		copy_vn_i(edge_users, numEdges, INVALID_UNUSED);
#endif

		for (eidx = 0, ed = orig_medge; eidx < numEdges; eidx++, ed++) {
			edge_users[eidx] = INVALID_UNUSED;
		}

		for (i = 0, mp = orig_mpoly; i < numFaces; i++, mp++) {
			MLoop *ml_prev;
			int j;

			ml = orig_mloop + mp->loopstart;
			ml_prev = ml + (mp->totloop - 1);

			for (j = 0; j < mp->totloop; j++, ml++) {
				/* add edge user */
				eidx = ml_prev->e;
				if (edge_users[eidx] == INVALID_UNUSED) {
					ed = orig_medge + eidx;
					BLI_assert(ELEM(ml_prev->v,    ed->v1, ed->v2) &&
					           ELEM(ml->v, ed->v1, ed->v2));
					edge_users[eidx] = (ml_prev->v > ml->v) == (ed->v1 < ed->v2) ? i : (i + numFaces);
					edge_order[eidx] = j;
				}
				else {
					edge_users[eidx] = INVALID_PAIR;
				}
				ml_prev = ml;
			}
		}

		for (eidx = 0, ed = orig_medge; eidx < numEdges; eidx++, ed++) {
			if (!ELEM(edge_users[eidx], INVALID_UNUSED, INVALID_PAIR)) {
				BLI_BITMAP_ENABLE(orig_mvert_tag, ed->v1);
				BLI_BITMAP_ENABLE(orig_mvert_tag, ed->v2);
				STACK_PUSH(new_edge_arr, eidx);
				newFaces++;
				newLoops += 4;
			}
		}

		for (i = 0; i < numVerts; i++) {
			if (BLI_BITMAP_TEST(orig_mvert_tag, i)) {
				old_vert_arr[i] = STACK_SIZE(new_vert_arr);
				STACK_PUSH(new_vert_arr, i);
				rimVerts++;
			}
			else {
				old_vert_arr[i] = INVALID_UNUSED;
			}
		}

		MEM_freeN(orig_mvert_tag);
	}

	if (do_shell == false) {
		/* only add rim vertices */
		newVerts = rimVerts;
		/* each extruded face needs an opposite edge */
		newEdges = newFaces;
	}
	else {
		/* (stride == 2) in this case, so no need to add newVerts/newEdges */
		BLI_assert(newVerts == 0);
		BLI_assert(newEdges == 0);
	}

	if (smd->flag & MOD_SOLIDIFY_NORMAL_CALC) {
		vert_nors = MEM_callocN(sizeof(float) * (size_t)numVerts * 3, "mod_solid_vno_hq");
		dm_calc_normal(dm, face_nors, vert_nors);
	}

	result = CDDM_from_template(dm,
	                            (int)((numVerts * stride) + newVerts),
	                            (int)((numEdges * stride) + newEdges + rimVerts), 0,
	                            (int)((numLoops * stride) + newLoops),
	                            (int)((numFaces * stride) + newFaces));

	mpoly = CDDM_get_polys(result);
	mloop = CDDM_get_loops(result);
	medge = CDDM_get_edges(result);
	mvert = CDDM_get_verts(result);

	if (do_shell) {
		DM_copy_vert_data(dm, result, 0, 0, (int)numVerts);
		DM_copy_vert_data(dm, result, 0, (int)numVerts, (int)numVerts);

		DM_copy_edge_data(dm, result, 0, 0, (int)numEdges);
		DM_copy_edge_data(dm, result, 0, (int)numEdges, (int)numEdges);

		DM_copy_loop_data(dm, result, 0, 0, (int)numLoops);
		DM_copy_loop_data(dm, result, 0, (int)numLoops, (int)numLoops);

		DM_copy_poly_data(dm, result, 0, 0, (int)numFaces);
		DM_copy_poly_data(dm, result, 0, (int)numFaces, (int)numFaces);
	}
	else {
		int i, j;
		DM_copy_vert_data(dm, result, 0, 0, (int)numVerts);
		for (i = 0, j = (int)numVerts; i < numVerts; i++) {
			if (old_vert_arr[i] != INVALID_UNUSED) {
				DM_copy_vert_data(dm, result, i, j, 1);
				j++;
			}
		}

		DM_copy_edge_data(dm, result, 0, 0, (int)numEdges);

		for (i = 0, j = (int)numEdges; i < numEdges; i++) {
			if (!ELEM(edge_users[i], INVALID_UNUSED, INVALID_PAIR)) {
				MEdge *ed_src, *ed_dst;
				DM_copy_edge_data(dm, result, i, j, 1);

				ed_src = &medge[i];
				ed_dst = &medge[j];
				ed_dst->v1 = old_vert_arr[ed_src->v1] + numVerts;
				ed_dst->v2 = old_vert_arr[ed_src->v2] + numVerts;
				j++;
			}
		}

		/* will be created later */
		DM_copy_loop_data(dm, result, 0, 0, (int)numLoops);
		DM_copy_poly_data(dm, result, 0, 0, (int)numFaces);
	}

#undef INVALID_UNUSED
#undef INVALID_PAIR


	/* initializes: (i_end, do_shell_align, mv)  */
#define INIT_VERT_ARRAY_OFFSETS(test) \
	if (((ofs_new >= ofs_orig) == do_flip) == test) { \
		i_end = numVerts; \
		do_shell_align = true; \
		mv = mvert; \
	} \
	else { \
		if (do_shell) { \
			i_end = numVerts; \
			do_shell_align = true; \
		} \
		else { \
			i_end = newVerts ; \
			do_shell_align = false; \
		} \
		mv = &mvert[numVerts]; \
	} (void)0


	/* flip normals */

	if (do_shell) {
		unsigned int i;

		mp = mpoly + numFaces;
		for (i = 0; i < dm->numPolyData; i++, mp++) {
			MLoop *ml2;
			unsigned int e;
			int j;

			/* reverses the loop direction (MLoop.v as well as custom-data)
			 * MLoop.e also needs to be corrected too, done in a separate loop below. */
			ml2 = mloop + mp->loopstart + dm->numLoopData;
			for (j = 0; j < mp->totloop; j++) {
				CustomData_copy_data(&dm->loopData, &result->loopData, mp->loopstart + j,
				                     mp->loopstart + (mp->totloop - j - 1) + dm->numLoopData, 1);
			}

			if (mat_ofs) {
				mp->mat_nr += mat_ofs;
				CLAMP(mp->mat_nr, 0, mat_nr_max);
			}

			e = ml2[0].e;
			for (j = 0; j < mp->totloop - 1; j++) {
				ml2[j].e = ml2[j + 1].e;
			}
			ml2[mp->totloop - 1].e = e;

			mp->loopstart += dm->numLoopData;

			for (j = 0; j < mp->totloop; j++) {
				ml2[j].e += numEdges;
				ml2[j].v += numVerts;
			}
		}

		for (i = 0, ed = medge + numEdges; i < numEdges; i++, ed++) {
			ed->v1 += numVerts;
			ed->v2 += numVerts;
		}
	}

	/* note, copied vertex layers don't have flipped normals yet. do this after applying offset */
	if ((smd->flag & MOD_SOLIDIFY_EVEN) == 0) {
		/* no even thickness, very simple */
		float scalar_short;
		float scalar_short_vgroup;

		/* for clamping */
		float *vert_lens = NULL;
		const float offset    = fabsf(smd->offset) * smd->offset_clamp;
		const float offset_sq = offset * offset;

		if (do_clamp) {
			unsigned int i;

			vert_lens = MEM_mallocN(sizeof(float) * numVerts, "vert_lens");
			copy_vn_fl(vert_lens, (int)numVerts, FLT_MAX);
			for (i = 0; i < numEdges; i++) {
				const float ed_len_sq = len_squared_v3v3(mvert[medge[i].v1].co, mvert[medge[i].v2].co);
				vert_lens[medge[i].v1] = min_ff(vert_lens[medge[i].v1], ed_len_sq);
				vert_lens[medge[i].v2] = min_ff(vert_lens[medge[i].v2], ed_len_sq);
			}
		}

		if (ofs_new != 0.0f) {
			unsigned int i_orig, i_end;
			bool do_shell_align;

			scalar_short = scalar_short_vgroup = ofs_new / 32767.0f;

			INIT_VERT_ARRAY_OFFSETS(false);

			for (i_orig = 0; i_orig < i_end; i_orig++, mv++) {
				const unsigned int i = do_shell_align ? i_orig : new_vert_arr[i_orig];
				if (dvert) {
					MDeformVert *dv = &dvert[i];
					if (defgrp_invert) scalar_short_vgroup = 1.0f - defvert_find_weight(dv, defgrp_index);
					else scalar_short_vgroup = defvert_find_weight(dv, defgrp_index);
					scalar_short_vgroup = (offset_fac_vg + (scalar_short_vgroup * offset_fac_vg_inv)) * scalar_short;
				}
				if (do_clamp) {
					/* always reset becaise we may have set before */
					if (dvert == NULL) {
						scalar_short_vgroup = scalar_short;
					}
					if (vert_lens[i] < offset_sq) {
						float scalar = sqrtf(vert_lens[i]) / offset;
						scalar_short_vgroup *= scalar;
					}
				}
				madd_v3v3short_fl(mv->co, mv->no, scalar_short_vgroup);
			}
		}

		if (ofs_orig != 0.0f) {
			unsigned int i_orig, i_end;
			bool do_shell_align;

			scalar_short = scalar_short_vgroup = ofs_orig / 32767.0f;

			/* as above but swapped */
			INIT_VERT_ARRAY_OFFSETS(true);

			for (i_orig = 0; i_orig < i_end; i_orig++, mv++) {
				const unsigned int i = do_shell_align ? i_orig : new_vert_arr[i_orig];
				if (dvert) {
					MDeformVert *dv = &dvert[i];
					if (defgrp_invert) scalar_short_vgroup = 1.0f - defvert_find_weight(dv, defgrp_index);
					else scalar_short_vgroup = defvert_find_weight(dv, defgrp_index);
					scalar_short_vgroup = (offset_fac_vg + (scalar_short_vgroup * offset_fac_vg_inv)) * scalar_short;
				}
				if (do_clamp) {
					/* always reset becaise we may have set before */
					if (dvert == NULL) {
						scalar_short_vgroup = scalar_short;
					}
					if (vert_lens[i] < offset_sq) {
						float scalar = sqrtf(vert_lens[i]) / offset;
						scalar_short_vgroup *= scalar;
					}
				}
				madd_v3v3short_fl(mv->co, mv->no, scalar_short_vgroup);
			}
		}

		if (do_clamp) {
			MEM_freeN(vert_lens);
		}
	}
	else {
#ifdef USE_NONMANIFOLD_WORKAROUND
		const bool check_non_manifold = (smd->flag & MOD_SOLIDIFY_NORMAL_CALC) != 0;
#endif
		/* same as EM_solidify() in editmesh_lib.c */
		float *vert_angles = MEM_callocN(sizeof(float) * numVerts * 2, "mod_solid_pair"); /* 2 in 1 */
		float *vert_accum = vert_angles + numVerts;
		unsigned int vidx;
		unsigned int i;

		if (vert_nors == NULL) {
			vert_nors = MEM_mallocN(sizeof(float) * numVerts * 3, "mod_solid_vno");
			for (i = 0, mv = mvert; i < numVerts; i++, mv++) {
				normal_short_to_float_v3(vert_nors[i], mv->no);
			}
		}

		for (i = 0, mp = mpoly; i < numFaces; i++, mp++) {
			/* #BKE_mesh_calc_poly_angles logic is inlined here */
			float nor_prev[3];
			float nor_next[3];

			int i_curr = mp->totloop - 1;
			int i_next = 0;

			ml = &mloop[mp->loopstart];

			sub_v3_v3v3(nor_prev, mvert[ml[i_curr - 1].v].co, mvert[ml[i_curr].v].co);
			normalize_v3(nor_prev);

			while (i_next < mp->totloop) {
				float angle;
				sub_v3_v3v3(nor_next, mvert[ml[i_curr].v].co, mvert[ml[i_next].v].co);
				normalize_v3(nor_next);
				angle = angle_normalized_v3v3(nor_prev, nor_next);


				/* --- not related to angle calc --- */
				if (angle < FLT_EPSILON) {
					angle = FLT_EPSILON;
				}

				vidx = ml[i_curr].v;
				vert_accum[vidx] += angle;

#ifdef USE_NONMANIFOLD_WORKAROUND
				/* skip 3+ face user edges */
				if ((check_non_manifold == false) ||
				    LIKELY(((orig_medge[ml[i_curr].e].flag & ME_EDGE_TMP_TAG) == 0) &&
				           ((orig_medge[ml[i_next].e].flag & ME_EDGE_TMP_TAG) == 0)))
				{
					vert_angles[vidx] += shell_v3v3_normalized_to_dist(vert_nors[vidx], face_nors[i]) * angle;
				}
				else {
					vert_angles[vidx] += angle;
				}
#else
				vert_angles[vidx] += shell_v3v3_normalized_to_dist(vert_nors[vidx], face_nors[i]) * angle;
#endif
				/* --- end non-angle-calc section --- */


				/* step */
				copy_v3_v3(nor_prev, nor_next);
				i_curr = i_next;
				i_next++;
			}
		}

		/* vertex group support */
		if (dvert) {
			MDeformVert *dv = dvert;
			float scalar;

			if (defgrp_invert) {
				for (i = 0; i < numVerts; i++, dv++) {
					scalar = 1.0f - defvert_find_weight(dv, defgrp_index);
					scalar = offset_fac_vg + (scalar * offset_fac_vg_inv);
					vert_angles[i] *= scalar;
				}
			}
			else {
				for (i = 0; i < numVerts; i++, dv++) {
					scalar = defvert_find_weight(dv, defgrp_index);
					scalar = offset_fac_vg + (scalar * offset_fac_vg_inv);
					vert_angles[i] *= scalar;
				}
			}
		}

		if (do_clamp) {
			float *vert_lens_sq = MEM_mallocN(sizeof(float) * numVerts, "vert_lens");
			const float offset    = fabsf(smd->offset) * smd->offset_clamp;
			const float offset_sq = offset * offset;
			copy_vn_fl(vert_lens_sq, (int)numVerts, FLT_MAX);
			for (i = 0; i < numEdges; i++) {
				const float ed_len = len_squared_v3v3(mvert[medge[i].v1].co, mvert[medge[i].v2].co);
				vert_lens_sq[medge[i].v1] = min_ff(vert_lens_sq[medge[i].v1], ed_len);
				vert_lens_sq[medge[i].v2] = min_ff(vert_lens_sq[medge[i].v2], ed_len);
			}
			for (i = 0; i < numVerts; i++) {
				if (vert_lens_sq[i] < offset_sq) {
					float scalar = sqrtf(vert_lens_sq[i]) / offset;
					vert_angles[i] *= scalar;
				}
			}
			MEM_freeN(vert_lens_sq);
		}

		if (ofs_new != 0.0f) {
			unsigned int i_orig, i_end;
			bool do_shell_align;

			INIT_VERT_ARRAY_OFFSETS(false);

			for (i_orig = 0; i_orig < i_end; i_orig++, mv++) {
				const unsigned int i_other = do_shell_align ? i_orig : new_vert_arr[i_orig];
				if (vert_accum[i_other]) { /* zero if unselected */
					madd_v3_v3fl(mv->co, vert_nors[i_other], ofs_new * (vert_angles[i_other] / vert_accum[i_other]));
				}
			}
		}

		if (ofs_orig != 0.0f) {
			unsigned int i_orig, i_end;
			bool do_shell_align;

			/* same as above but swapped, intentional use of 'ofs_new' */
			INIT_VERT_ARRAY_OFFSETS(true);

			for (i_orig = 0; i_orig < i_end; i_orig++, mv++) {
				const unsigned int i_other = do_shell_align ? i_orig : new_vert_arr[i_orig];
				if (vert_accum[i_other]) { /* zero if unselected */
					madd_v3_v3fl(mv->co, vert_nors[i_other], ofs_orig * (vert_angles[i_other] / vert_accum[i_other]));
				}
			}
		}

		MEM_freeN(vert_angles);
	}

	if (vert_nors)
		MEM_freeN(vert_nors);

	/* must recalculate normals with vgroups since they can displace unevenly [#26888] */
	if ((dm->dirty & DM_DIRTY_NORMALS) || (smd->flag & MOD_SOLIDIFY_RIM) || dvert) {
		result->dirty |= DM_DIRTY_NORMALS;
	}
	else if (do_shell) {
		unsigned int i;
		/* flip vertex normals for copied verts */
		mv = mvert + numVerts;
		for (i = 0; i < numVerts; i++, mv++) {
			negate_v3_short(mv->no);
		}
	}

	if (smd->flag & MOD_SOLIDIFY_RIM) {
		unsigned int i;

		/* bugger, need to re-calculate the normals for the new edge faces.
		 * This could be done in many ways, but probably the quickest way
		 * is to calculate the average normals for side faces only.
		 * Then blend them with the normals of the edge verts.
		 *
		 * at the moment its easiest to allocate an entire array for every vertex,
		 * even though we only need edge verts - campbell
		 */

#define SOLIDIFY_SIDE_NORMALS

#ifdef SOLIDIFY_SIDE_NORMALS
		const bool do_side_normals = !(result->dirty & DM_DIRTY_NORMALS);
		/* annoying to allocate these since we only need the edge verts, */
		float (*edge_vert_nos)[3] = do_side_normals ? MEM_callocN(sizeof(float) * numVerts * 3, __func__) : NULL;
		float nor[3];
#endif
		const unsigned char crease_rim = smd->crease_rim * 255.0f;
		const unsigned char crease_outer = smd->crease_outer * 255.0f;
		const unsigned char crease_inner = smd->crease_inner * 255.0f;

		int *origindex_edge;
		int *orig_ed;
		unsigned int j;

		if (crease_rim || crease_outer || crease_inner) {
			result->cd_flag |= ME_CDFLAG_EDGE_CREASE;
		}

		/* add faces & edges */
		origindex_edge = result->getEdgeDataArray(result, CD_ORIGINDEX);
		ed = &medge[(numEdges * stride) + newEdges];  /* start after copied edges */
		orig_ed = &origindex_edge[(numEdges * stride) + newEdges];
		for (i = 0; i < rimVerts; i++, ed++, orig_ed++) {
			ed->v1 = new_vert_arr[i];
			ed->v2 = (do_shell ? new_vert_arr[i] : i) + numVerts;
			ed->flag |= ME_EDGEDRAW;

			*orig_ed = ORIGINDEX_NONE;

			if (crease_rim) {
				ed->crease = crease_rim;
			}
		}

		/* faces */
		mp = mpoly + (numFaces * stride);
		ml = mloop + (numLoops * stride);
		j = 0;
		for (i = 0; i < newFaces; i++, mp++) {
			unsigned int eidx = new_edge_arr[i];
			unsigned int fidx = edge_users[eidx];
			int k1, k2;
			bool flip;

			if (fidx >= numFaces) {
				fidx -= numFaces;
				flip = true;
			}
			else {
				flip = false;
			}

			ed = medge + eidx;

			/* copy most of the face settings */
			DM_copy_poly_data(dm, result, (int)fidx, (int)((numFaces * stride) + i), 1);
			mp->loopstart = (int)(j + (numLoops * stride));
			mp->flag = mpoly[fidx].flag;

			/* notice we use 'mp->totloop' which is later overwritten,
			 * we could lookup the original face but theres no point since this is a copy
			 * and will have the same value, just take care when changing order of assignment */
			k1 = mpoly[fidx].loopstart + (((edge_order[eidx] - 1) + mp->totloop) % mp->totloop);  /* prev loop */
			k2 = mpoly[fidx].loopstart +   (edge_order[eidx]);

			mp->totloop = 4;

			CustomData_copy_data(&dm->loopData, &result->loopData, k2, (int)((numLoops * stride) + j + 0), 1);
			CustomData_copy_data(&dm->loopData, &result->loopData, k1, (int)((numLoops * stride) + j + 1), 1);
			CustomData_copy_data(&dm->loopData, &result->loopData, k1, (int)((numLoops * stride) + j + 2), 1);
			CustomData_copy_data(&dm->loopData, &result->loopData, k2, (int)((numLoops * stride) + j + 3), 1);

			if (flip == false) {
				ml[j].v = ed->v1;
				ml[j++].e = eidx;

				ml[j].v = ed->v2;
				ml[j++].e = (numEdges * stride) + old_vert_arr[ed->v2] + newEdges;

				ml[j].v = (do_shell ? ed->v2 : old_vert_arr[ed->v2]) + numVerts;
				ml[j++].e = (do_shell ? eidx : i) + numEdges;

				ml[j].v = (do_shell ? ed->v1 : old_vert_arr[ed->v1]) + numVerts;
				ml[j++].e = (numEdges * stride) + old_vert_arr[ed->v1] + newEdges;
			}
			else {
				ml[j].v = ed->v2;
				ml[j++].e = eidx;

				ml[j].v = ed->v1;
				ml[j++].e = (numEdges * stride) + old_vert_arr[ed->v1] + newEdges;

				ml[j].v = (do_shell ? ed->v1 : old_vert_arr[ed->v1]) + numVerts;
				ml[j++].e = (do_shell ? eidx : i) + numEdges;

				ml[j].v = (do_shell ? ed->v2 : old_vert_arr[ed->v2]) + numVerts;
				ml[j++].e = (numEdges * stride) + old_vert_arr[ed->v2] + newEdges;
			}

			origindex_edge[ml[j - 3].e] = ORIGINDEX_NONE;
			origindex_edge[ml[j - 1].e] = ORIGINDEX_NONE;

			/* use the next material index if option enabled */
			if (mat_ofs_rim) {
				mp->mat_nr += mat_ofs_rim;
				CLAMP(mp->mat_nr, 0, mat_nr_max);
			}
			if (crease_outer) {
				/* crease += crease_outer; without wrapping */
				char *cr = &(ed->crease);
				int tcr = *cr + crease_outer;
				*cr = tcr > 255 ? 255 : tcr;
			}

			if (crease_inner) {
				/* crease += crease_inner; without wrapping */
				char *cr = &(medge[numEdges + (do_shell ? eidx : i)].crease);
				int tcr = *cr + crease_inner;
				*cr = tcr > 255 ? 255 : tcr;
			}

#ifdef SOLIDIFY_SIDE_NORMALS
			if (do_side_normals) {
				normal_quad_v3(nor,
				               mvert[ml[j - 4].v].co,
				               mvert[ml[j - 3].v].co,
				               mvert[ml[j - 2].v].co,
				               mvert[ml[j - 1].v].co);

				add_v3_v3(edge_vert_nos[ed->v1], nor);
				add_v3_v3(edge_vert_nos[ed->v2], nor);
			}
#endif
		}

#ifdef SOLIDIFY_SIDE_NORMALS
		if (do_side_normals) {
			ed = medge + (numEdges * stride);
			for (i = 0; i < rimVerts; i++, ed++) {
				float nor_cpy[3];
				short *nor_short;
				int k;

				/* note, only the first vertex (lower half of the index) is calculated */
				normalize_v3_v3(nor_cpy, edge_vert_nos[ed->v1]);

				for (k = 0; k < 2; k++) { /* loop over both verts of the edge */
					nor_short = mvert[*(&ed->v1 + k)].no;
					normal_short_to_float_v3(nor, nor_short);
					add_v3_v3(nor, nor_cpy);
					normalize_v3(nor);
					normal_float_to_short_v3(nor_short, nor);
				}
			}

			MEM_freeN(edge_vert_nos);
		}
#endif

		MEM_freeN(new_vert_arr);
		MEM_freeN(new_edge_arr);

		MEM_freeN(edge_users);
		MEM_freeN(edge_order);
	}

	if (old_vert_arr)
		MEM_freeN(old_vert_arr);

	if (face_nors)
		MEM_freeN(face_nors);

	if (numFaces == 0 && numEdges != 0) {
		modifier_setError(md, "Faces needed for useful output");
	}

	return result;
}
Ejemplo n.º 23
0
static MDefBoundIsect *meshdeform_ray_tree_intersect(MeshDeformBind *mdb, const float co1[3], const float co2[3])
{
	BVHTreeRayHit hit;
	MeshDeformIsect isect_mdef;
	struct MeshRayCallbackData data = {
		mdb,
		&isect_mdef,
	};
	float end[3], vec_normal[3];

	/* happens binding when a cage has no faces */
	if (UNLIKELY(mdb->bvhtree == NULL))
		return NULL;

	/* setup isec */
	memset(&isect_mdef, 0, sizeof(isect_mdef));
	isect_mdef.lambda = 1e10f;

	copy_v3_v3(isect_mdef.start, co1);
	copy_v3_v3(end, co2);
	sub_v3_v3v3(isect_mdef.vec, end, isect_mdef.start);
	isect_mdef.vec_length = normalize_v3_v3(vec_normal, isect_mdef.vec);

	hit.index = -1;
	hit.dist = BVH_RAYCAST_DIST_MAX;
	if (BLI_bvhtree_ray_cast(mdb->bvhtree, isect_mdef.start, vec_normal,
	                         0.0, &hit, harmonic_ray_callback, &data) != -1)
	{
		const MLoop *mloop = mdb->cagedm_cache.mloop;
		const MLoopTri *lt = &mdb->cagedm_cache.looptri[hit.index];
		const MPoly *mp = &mdb->cagedm_cache.mpoly[lt->poly];
		const float (*cagecos)[3] = mdb->cagecos;
		const float len = isect_mdef.lambda;
		MDefBoundIsect *isect;

		float (*mp_cagecos)[3] = BLI_array_alloca(mp_cagecos, mp->totloop);
		int i;

		/* create MDefBoundIsect, and extra for 'poly_weights[]' */
		isect = BLI_memarena_alloc(mdb->memarena, sizeof(*isect) + (sizeof(float) * mp->totloop));

		/* compute intersection coordinate */
		madd_v3_v3v3fl(isect->co, co1, isect_mdef.vec, len);

		isect->facing = isect_mdef.isect;

		isect->poly_index = lt->poly;

		isect->len = max_ff(len_v3v3(co1, isect->co), MESHDEFORM_LEN_THRESHOLD);

		/* compute mean value coordinates for interpolation */
		for (i = 0; i < mp->totloop; i++) {
			copy_v3_v3(mp_cagecos[i], cagecos[mloop[mp->loopstart + i].v]);
		}

		interp_weights_poly_v3(isect->poly_weights, mp_cagecos, mp->totloop, isect->co);

		return isect;
	}

	return NULL;
}
Ejemplo n.º 24
0
/* 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);
}