cpu_sph.c

来自「游戏编程精粹６第中关于粒子的实时流体仿真系统,对入门的游戏开发者很有帮助．」· C语言 代码 · 共 741 行 · 第 1/2 页

				vec3_sub(&vdiff, &VEL(nindex), &VEL(i));

/*
				if ((i < sph->n_rigidp[0]) || (nindex < sph->n_rigidp[0]))
				vec3_scale(&vdiff, 0.0f*sph->viscosity*sph->lap_vis_coef, &vdiff);
				else vec3_scale(&vdiff, sph->viscosity*sph->lap_vis_coef, &vdiff);
				*/
vec3_scale(&vdiff, sph->viscosity*sph->lap_vis_coef, &vdiff);

				vec3_add(&force, &force, &vdiff);


				vec3_scale(&force, h_r*sph->density[i]*sph->density[nindex], &force);

				// Apply force
				vec3_scaleadd(&ACC(i), &ACC(i), sph->mass[nindex], &force);
				vec3_scaleadd(&ACC(nindex), &ACC(nindex), -sph->mass[i], &force);

			}
		}

}


_inline void compute_col(vector3* p, vector3* col,
						 const vector3* vel, const vector3* n, 
						 float diff, float stiff, float damp)
{
	float v0;
	float reverse;

	v0 = vec3_dot(n, vel);
#ifdef PUSHOUT
	//mat4_mulvec3(&n_w, &mat_col, &n);
	vec3_scaleadd(p, p, diff, n);
#endif
#ifdef REFLECT_VEL
	vec3_scaleadd(col, col, -1.9*v0, n);
#else
	reverse = stiff*diff - damp*v0;

	vec3_scaleadd(col, col, reverse, n);
#endif
}
/**
 * col is ref_v if REFLECT_VEL is defined, col_force if REFLECT_VEL is not defined
 */


_inline void cpu_sph_glass_collision(vector3* p, vector3* col, const vector3* vel,
									 const matrix4* mat, const matrix4* mat_inv, 
									 float radius, float stiff, float damp)
{
	float GLASS_R = 0.05f;
	//		float GLASS_R = 0.045;
	float GLASS_BOTTOM = -0.08f;
#ifdef POOR_WATER
	float GLASS_TOP = 0.04f;
#else
	float GLASS_TOP = 0.06f;
	//float GLASS_TOP = 0.18;
	//float GLASS_TOP = 0.26;
#endif
	float GLASS_THICKNESS = 0.01f;

	vector3 p_col;
	vector3 n;
	float diff; /** Distance between object and fluid particle **/

	mat4_mulvec3(&p_col, mat_inv, p);

	diff =
        2.0f*radius - (GLASS_R - (float)sqrt(p_col.x*p_col.x + p_col.y*p_col.y));

	if (((diff < 8.0f*radius) && (diff > EPSILON)) && (p_col.z < GLASS_TOP))
	{
		vec3_set(&n, -p_col.x, -p_col.y, 0.0f);
		vec3_normalize(&n, &n);

		mat4_mulvec3_as_mat3(&n, mat, &n);
		compute_col(p, col, vel, &n, diff, stiff, damp);
	}

	diff = 2.0f*radius - (p_col.z - GLASS_BOTTOM);

	if (diff > EPSILON)
	{
		vec3_set(&n, 0.0f, 0.0f, 1.0f);

		mat4_mulvec3_as_mat3(&n, mat, &n);
		compute_col(p, col, vel, &n, diff, stiff, damp);
	}

#ifndef POOR_WATER
	diff = 2.0f*radius - (GLASS_TOP - p_col.z);

	if (diff > EPSILON)
	{
		vec3_set(&n, 0.0f, 0.0f, -1.0f);;

		mat4_mulvec3_as_mat3(&n, mat, &n);
		compute_col(p, col, vel, &n, diff, stiff, damp);
	}
#endif
}

void cpu_sph_process_collision(cpu_sph* sph)
{
	int i;
	float e;
	float sphere_radius;
	float stiff;
	float damp;
//	const float EPSILON = 0.000001f;

//	stiff = 5000.0f;
	//stiff = 10000.0f;
	stiff = 30000.0f;
	damp = 128.0f;
	//damp = 64.0f;
	
	//sphere_radius = RADIUS;
	sphere_radius = 0.004f;
	e = 1.0f;


	for (i = 0; i < sph->n_particles; i++)
	{

		//float BOX_W = 0.08f; /** Width of box **/
		//float BOX_BOTTOM = -0.13f; /** Bottom of box **/
		float BOX_W = 0.065f; /** Width of box **/
		float BOX_BOTTOM = -0.08f; /** Bottom of box **/
//		_ALIGNED vector3 p; /** Position in collision coordinate **/
		//_ALIGNED vector3 vel; /** Velocity in collision coordinate **/

//        _ALIGNED vector3 n_w; /** Normal in world coordinate **/
		_ALIGNED vector3 pre_p; /** Predicted pos **/
		vector3 col;

#ifdef POOR_WATER
		vector3 n;
		float diff;
#endif

		vec3_set(&col, 0.0f, 0.0f, 0.0f);
		vec3_scaleadd(&pre_p, &POS(i), sph->timestep, &VELH(i));

#ifdef RIGID_BODY
		if (i < sph->n_rigidp[0])
			stiff = 50000.0f;
		else stiff = 30000.0f;
#endif

#ifdef GLASS
		cpu_sph_glass_collision(&pre_p, &col, &VEL(i),
			&sph->mat_col, &sph->mat_inv_col, sphere_radius, stiff, damp);
#endif
	

		/** Box **/
#ifdef POOR_WATER
		diff = 2.0f*sphere_radius - (pre_p.z - BOX_BOTTOM);
		if (diff > EPSILON)
		{
			vec3_set(&n, 0.0f, 0.0f, 1.0f);
			compute_col(&pre_p, &col, &VEL(i), &n, diff, stiff, damp);
		}

		diff = 2.0f*sphere_radius - (BOX_W - pre_p.x);
		if (diff > EPSILON)
		{
			vec3_set(&n, -1.0f, 0.0f, 0.0f);
			compute_col(&pre_p, &col, &VEL(i), &n, diff, stiff, damp);
		}

		diff = 2.0f*sphere_radius - (BOX_W + pre_p.x);
		if (diff > EPSILON)
		{
			vec3_set(&n, 1.0f, 0.0f, 0.0f);
			compute_col(&pre_p, &col, &VEL(i), &n, diff, stiff, damp);
		}

		diff = 2.0f*sphere_radius - (BOX_W - pre_p.y);
		if (diff > EPSILON)
		{
			vec3_set(&n, 0.0f, -1.0f, 0.0f);
			compute_col(&pre_p, &col, &VEL(i), &n, diff, stiff, damp);
		}

		diff = 2.0f*sphere_radius - (BOX_W + pre_p.y);
		if (diff > EPSILON)
		{
			vec3_set(&n, 0.0f, 1.0f, 0.0f);
			compute_col(&pre_p, &col, &VEL(i), &n, diff, stiff, damp);
		}
#endif

	
		vec3_add(&ACC(i), &ACC(i), &col);

	}

	return;
}


#ifdef RIGID_BODY
void cpu_sph_compute_rigid_body_motion(cpu_sph* sph, float t)
{
	int i;
	vector3 rg;
	vector3 T;
	vector3 R;
	vector3* q;
	vector3* temp_vel;
	vector3 g;
	float I;

	vec3_set(&g, 0.0f, 0.0f, -9.8f);

	q = (vector3*)malloc(sph->n_rigidp[0]*sizeof(vector3));
	temp_vel = (vector3*)malloc(sph->n_rigidp[0]*sizeof(vector3));

	// Compute new velocity
	FOR_EACH_RIGID(0, i)
	{
		_ALIGNED vector3 final_acc;

		vec3_add(&final_acc, &sph->acc[i], &g);
		vec3_scaleadd(&temp_vel[i], &VELH(i), t, &final_acc);
	}
	

	// Compute translational velocity
	vec3_set(&T, 0, 0, 0);
	FOR_EACH_RIGID(0, i)
		vec3_add(&T, &T, &temp_vel[i]);
	vec3_scale(&T, 1.0f/sph->n_rigidp[0], &T);
	//vec3_scaleadd(&T, &T, t, &g);

	// Update translational velocity
	//vec3_scaleadd(&sph->v_trans_half, &sph->v_trans_half, t, &T);

	// Compute center of gravity 
	vec3_set(&rg, 0.0f, 0.0f, 0.0f);
	FOR_EACH_RIGID(0, i)
		vec3_add(&rg, &rg, &POS(i));
	vec3_scale(&rg, 1.0f/sph->n_rigidp[0], &rg);

	// Compute angular velocity
	FOR_EACH_RIGID(0, i)
		vec3_sub(&q[i], &POS(i), &rg);

	I = 0;
	vec3_set(&R, 0.0f, 0.0f, 0.0f);
	FOR_EACH_RIGID(0, i)
	{
		vector3 c;
		I += vec3_lensq(&q[i]);

//		vec3_cross(&c, &temp_vel[i], &q[i]);
		vec3_cross(&c, &q[i], &temp_vel[i]);
		vec3_add(&R, &R, &c);
	}
	vec3_scale(&R, 1.0f/I, &R);

	
	// Compute rotation matrix
	{
		vector3 axis;
		matrix4 dr;
		float r = vec3_len(&R);
		vec3_scale(&axis, 1.0f/r, &R);
		mat4_set_rotate(&dr, t*r/M_PI*180.0f, axis.x, axis.y, axis.z);
		mat4_mul(&sph->r_rot, &dr, &sph->r_rot);
	}

	
	// Update velocity of each rigid body particles
	FOR_EACH_RIGID(0, i)
	{
		_ALIGNED vector3 v_half;

//		vec3_cross(&v_half, &q[i], &R);
		vec3_cross(&v_half, &R, &q[i]);
		vec3_add(&v_half, &v_half, &T);

		//vec3_scaleadd(&sph->pos[i], &sph->pos[i], t, &v_half);

		vec3_add(&VEL(i), &VELH(i), &v_half);
		vec3_scale(&VEL(i), 0.5f, &VEL(i));

		vec3_copy(&VELH(i), &v_half);
	}

	vec3_scaleadd(&rg, &rg, t, &T);

	
	FOR_EACH_RIGID(0, i)
	{
		_ALIGNED vector3 tempp;

		vec3_sub(&tempp, &sph->init_pos[i], &sph->rg0);
		mat4_mulvec3_as_mat3(&tempp, &sph->r_rot, &tempp);
		vec3_add(&POS(i), &tempp, &rg);
	}
	
	

	//vec3_scaleadd(&sph->r, &sph->r, t, &sph->w_half);
	vec3_copy(&sph->c, &rg);


	free(temp_vel);
	free(q);
}
#endif // RIGID_BODY



void cpu_sph_elapse(cpu_sph* sph, float t)
{
	int i;
	int j;
	_ALIGNED vector3 gravity;

	sph->timestep = t;

	vec3_set(&gravity, 0.0f, 0.0f, -9.8f);

	for (j = 0; j < sph->n_loops; j++)
	{
		if (j == 0)
		{
			sph_grid_clear(&sph->grid, sph->pos, 0, sph->n_particles - 1);
			sph_grid_alloc(&sph->grid, sph->pos, 0, sph->fluid_start - 1);
			sph_grid_get_neighbours(&sph->grid, sph->pos, sph->fluid_start, sph->n_particles - 1, &sph->n_list, sph->r_search);
		}


		cpu_sph_compute_density(sph, (j == 0));
		cpu_sph_compute_force(sph);
		cpu_sph_process_collision(sph);

#ifdef RIGID_BODY
		cpu_sph_compute_rigid_body_motion(sph, t);
#endif
		FOR_EACH_FLUID(i)
		{
			_ALIGNED vector3 v_half;
			_ALIGNED vector3 final_acc;

			vec3_add(&final_acc, &sph->acc[i], &gravity);

			vec3_scaleadd(&v_half, &VELH(i), t, &final_acc);
			vec3_scaleadd(&POS(i), &POS(i), t, &v_half);

			vec3_add(&VEL(i), &VELH(i), &v_half);
			vec3_scale(&VEL(i), 0.5f, &VEL(i));

			vec3_copy(&VELH(i), &v_half);
		}
	}
}

void cpu_sph_get_pos(cpu_sph* sph, vector3* pos)
{
	memcpy(pos, sph->pos, sph->n_particles*sizeof(vector3));
}