Particles.c

typedef struct ParticleSet {
	size_t count;

        // note that each of these arrays have to be aligned to the vector
        // width we'll be working with, since it always updates in multiples
        // of the vector width.
	float* x;
	float* y;
	float* z;
	float* vx;
	float* vy;
	float* vz;
} ParticleSet;

void particles_integrate(ParticleSet* set, vec4 plane, float delta) {
	__m128 dt = _mm_set1_ps(delta);

	__m128 px = _mm_set1_ps(plane.x);
	__m128 py = _mm_set1_ps(plane.y);
	__m128 pz = _mm_set1_ps(plane.z);
	__m128 pw = _mm_set1_ps(plane.w);

	for (size_t i = 0; i < set->count; i += 4) {
		__m128 x = _mm_load_ps(&set->x[i]);
		__m128 y = _mm_load_ps(&set->y[i]);
		__m128 z = _mm_load_ps(&set->z[i]);

		__m128 vx = _mm_load_ps(&set->vx[i]);
		__m128 vy = _mm_load_ps(&set->vy[i]);
		__m128 vz = _mm_load_ps(&set->vz[i]);

		vy = _mm_add_ps(vy, _mm_mul_ps(_mm_set1_ps(-10.0f), dt));
		_mm_store_ps(&set->vy[i], vy);

                // new position
		__m128 nx = _mm_add_ps(x, _mm_mul_ps(vx, dt));
		__m128 ny = _mm_add_ps(y, _mm_mul_ps(vy, dt));
		__m128 nz = _mm_add_ps(z, _mm_mul_ps(vz, dt));

		// dot(plane.XYZ, P) - plane.W
		__m128 rx = _mm_mul_ps(nx, px);
		__m128 ry = _mm_mul_ps(ny, py);
		__m128 rz = _mm_mul_ps(nz, pz);
		__m128 dist_to_plane = _mm_max_ps(_mm_add_ps(_mm_add_ps(rx, ry), _mm_sub_ps(rz, pw)), _mm_set1_ps(0.0f));

		// new position = new position - (plane.XYZ * dist_to_plane)
		nx = _mm_sub_ps(nx, _mm_mul_ps(px, _mm_shuffle_ps(dist_to_plane, dist_to_plane, 0x00)));
		ny = _mm_sub_ps(ny, _mm_mul_ps(py, _mm_shuffle_ps(dist_to_plane, dist_to_plane, 0x55)));
		nz = _mm_sub_ps(nz, _mm_mul_ps(pz, _mm_shuffle_ps(dist_to_plane, dist_to_plane, 0xAA)));

                // store these back
		_mm_store_ps(&set->x[i], nx);
		_mm_store_ps(&set->y[i], ny);
		_mm_store_ps(&set->z[i], nz);
	}
}