diff --git a/src/gui/game/GameModel.cpp b/src/gui/game/GameModel.cpp
index 5bc4203035..193e7ea277 100644
--- a/src/gui/game/GameModel.cpp
+++ b/src/gui/game/GameModel.cpp
@@ -174,7 +174,7 @@ GameModel::~GameModel()
 	Client::Ref().SetPref("Renderer.DebugMode", ren->debugLines); //These two should always be equivalent, even though they are different things
 
 	Client::Ref().SetPref("Simulation.EdgeMode", edgeMode);
-	Client::Ref().SetPref("Simulation.NewtonianGravity", sim->grav->ngrav_enable);
+	Client::Ref().SetPref("Simulation.NewtonianGravity", sim->grav->IsEnabled());
 	Client::Ref().SetPref("Simulation.AmbientHeat", sim->aheat_enable);
 	Client::Ref().SetPref("Simulation.PrettyPowder", sim->pretty_powder);
 
@@ -719,11 +719,11 @@ void GameModel::SetSaveFile(SaveFile * newSave, bool invertIncludePressure)
 		sim->legacy_enable = saveData->legacyEnable;
 		sim->water_equal_test = saveData->waterEEnabled;
 		sim->aheat_enable = saveData->aheatEnable;
-		if(saveData->gravityEnable && !sim->grav->ngrav_enable)
+		if(saveData->gravityEnable && !sim->grav->IsEnabled())
 		{
 			sim->grav->start_grav_async();
 		}
-		else if(!saveData->gravityEnable && sim->grav->ngrav_enable)
+		else if(!saveData->gravityEnable && sim->grav->IsEnabled())
 		{
 			sim->grav->stop_grav_async();
 		}
@@ -995,7 +995,7 @@ void GameModel::SetNewtonianGravity(bool newtonainGravity)
 
 bool GameModel::GetNewtonianGrvity()
 {
-    return sim->grav->ngrav_enable;
+    return sim->grav->IsEnabled();
 }
 
 void GameModel::ShowGravityGrid(bool showGrid)
diff --git a/src/gui/options/OptionsModel.cpp b/src/gui/options/OptionsModel.cpp
index 1a79e309ca..6284f94e92 100644
--- a/src/gui/options/OptionsModel.cpp
+++ b/src/gui/options/OptionsModel.cpp
@@ -46,7 +46,7 @@ void OptionsModel::SetAmbientHeatSimulation(bool state)
 
 bool OptionsModel::GetNewtonianGravity()
 {
-	return sim->grav->ngrav_enable?true:false;
+	return sim->grav->IsEnabled();
 }
 
 void OptionsModel::SetNewtonianGravity(bool state)
diff --git a/src/lua/LegacyLuaAPI.cpp b/src/lua/LegacyLuaAPI.cpp
index e82fb9adc4..ae8e4bd490 100644
--- a/src/lua/LegacyLuaAPI.cpp
+++ b/src/lua/LegacyLuaAPI.cpp
@@ -1226,7 +1226,7 @@ int luatpt_gravity(lua_State* l)
 	int acount = lua_gettop(l);
 	if (acount == 0)
 	{
-		lua_pushinteger(l, luacon_sim->grav->ngrav_enable);
+		lua_pushinteger(l, luacon_sim->grav->IsEnabled() ? 1 : 0);
 		return 1;
 	}
 	int gravstate = luaL_checkint(l, 1);
diff --git a/src/simulation/Gravity.cpp b/src/simulation/Gravity.cpp
index 455c46c75c..e37e01f35d 100755
--- a/src/simulation/Gravity.cpp
+++ b/src/simulation/Gravity.cpp
@@ -1,160 +1,198 @@
 #include "Gravity.h"
 
-#include "SimulationData.h"
-
 #include <cmath>
+#include <iostream>
 #include <sys/types.h>
 
+#include "CoordStack.h"
 #include "Misc.h"
-
 #include "Simulation.h"
+#include "SimulationData.h"
 
-void Gravity::bilinear_interpolation(float *src, float *dst, int sw, int sh, int rw, int rh)
+
+Gravity::Gravity()
 {
-	int y, x, fxceil, fyceil;
-	float fx, fy, fyc, fxc;
-	double intp;
-	float tr, tl, br, bl;
-	//Bilinear interpolation for upscaling
-	for (y=0; y<rh; y++)
-		for (x=0; x<rw; x++)
-		{
-			fx = ((float)x)*((float)sw)/((float)rw);
-			fy = ((float)y)*((float)sh)/((float)rh);
-			fxc = modf(fx, &intp);
-			fyc = modf(fy, &intp);
-			fxceil = (int)ceil(fx);
-			fyceil = (int)ceil(fy);
-			if (fxceil>=sw) fxceil = sw-1;
-			if (fyceil>=sh) fyceil = sh-1;
-			tr = src[sw*(int)floor(fy)+fxceil];
-			tl = src[sw*(int)floor(fy)+(int)floor(fx)];
-			br = src[sw*fyceil+fxceil];
-			bl = src[sw*fyceil+(int)floor(fx)];
-			dst[rw*y+x] = ((tl*(1.0f-fxc))+(tr*(fxc)))*(1.0f-fyc) + ((bl*(1.0f-fxc))+(br*(fxc)))*(fyc);
-		}
+	// Allocate full size Gravmaps
+	unsigned int size = (XRES / CELL) * (YRES / CELL);
+	th_ogravmap = new float[size];
+	th_gravmap = new float[size];
+	th_gravy = new float[size];
+	th_gravx = new float[size];
+	th_gravp = new float[size];
+	gravmap = new float[size];
+	gravy = new float[size];
+	gravx = new float[size];
+	gravp = new float[size];
+	gravmask = new unsigned[size];
+}
+
+Gravity::~Gravity()
+{
+	stop_grav_async();
+#ifdef GRAVFFT
+	grav_fft_cleanup();
+#endif
+
+	delete[] th_ogravmap;
+	delete[] th_gravmap;
+	delete[] th_gravy;
+	delete[] th_gravx;
+	delete[] th_gravp;
+	delete[] gravmap;
+	delete[] gravy;
+	delete[] gravx;
+	delete[] gravp;
+	delete[] gravmask;
 }
 
-bool ignoreNextResult = false;
 void Gravity::Clear()
 {
-	std::fill(gravy, gravy+((XRES/CELL)*(YRES/CELL)), 0.0f);
-	std::fill(gravx, gravx+((XRES/CELL)*(YRES/CELL)), 0.0f);
-	std::fill(gravp, gravp+((XRES/CELL)*(YRES/CELL)), 0.0f);
-	std::fill(gravmap, gravmap+((XRES/CELL)*(YRES/CELL)), 0.0f);
-	std::fill(gravmask, gravmask+((XRES/CELL)*(YRES/CELL)), 0xFFFFFFFF);
+	int size = (XRES / CELL) * (YRES / CELL);
+	std::fill(gravy, gravy + size, 0.0f);
+	std::fill(gravx, gravx + size, 0.0f);
+	std::fill(gravp, gravp + size, 0.0f);
+	std::fill(gravmap, gravmap + size, 0.0f);
+	std::fill(gravmask, gravmask + size, 0xFFFFFFFF);
 
 	ignoreNextResult = true;
 }
 
-void Gravity::gravity_init()
+#ifdef GRAVFFT
+void Gravity::grav_fft_init()
 {
-	ngrav_enable = 0;
-	//Allocate full size Gravmaps
-	th_ogravmap = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	th_gravmap = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	th_gravy = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	th_gravx = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	th_gravp = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	gravmap = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	gravy = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	gravx = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	gravp = (float *)calloc((XRES/CELL)*(YRES/CELL), sizeof(float));
-	gravmask = (unsigned int *)calloc((XRES/CELL)*(YRES/CELL), sizeof(unsigned));
-	obmap = (unsigned char (*)[XRES/CELL])calloc((XRES/CELL)*(YRES/CELL), sizeof(unsigned char));
+	int xblock2 = XRES/CELL*2;
+	int yblock2 = YRES/CELL*2;
+	int fft_tsize = (xblock2/2+1)*yblock2;
+	float distance, scaleFactor;
+	fftwf_plan plan_ptgravx, plan_ptgravy;
+	if (grav_fft_status) return;
+
+	//use fftw malloc function to ensure arrays are aligned, to get better performance
+	th_ptgravx = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
+	th_ptgravy = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
+	th_ptgravxt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
+	th_ptgravyt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
+	th_gravmapbig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
+	th_gravmapbigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
+	th_gravxbig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
+	th_gravybig = reinterpret_cast<float*>(fftwf_malloc(xblock2 * yblock2 * sizeof(float)));
+	th_gravxbigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
+	th_gravybigt = reinterpret_cast<fftwf_complex*>(fftwf_malloc(fft_tsize * sizeof(fftwf_complex)));
+
+	//select best algorithm, could use FFTW_PATIENT or FFTW_EXHAUSTIVE but that increases the time taken to plan, and I don't see much increase in execution speed
+	plan_ptgravx = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravx, th_ptgravxt, FFTW_MEASURE);
+	plan_ptgravy = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravy, th_ptgravyt, FFTW_MEASURE);
+	plan_gravmap = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_gravmapbig, th_gravmapbigt, FFTW_MEASURE);
+	plan_gravx_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravxbigt, th_gravxbig, FFTW_MEASURE);
+	plan_gravy_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravybigt, th_gravybig, FFTW_MEASURE);
+
+	//(XRES/CELL)*(YRES/CELL)*4 is size of data array, scaling needed because FFTW calculates an unnormalized DFT
+	scaleFactor = -M_GRAV/((XRES/CELL)*(YRES/CELL)*4);
+	//calculate velocity map caused by a point mass
+	for (int y = 0; y < yblock2; y++)
+	{
+		for (int x = 0; x < xblock2; x++)
+		{
+			if (x == XRES / CELL && y == YRES / CELL)
+				continue;
+			distance = sqrtf(pow(x-(XRES/CELL), 2.0f) + pow(y-(YRES/CELL), 2.0f));
+			th_ptgravx[y * xblock2 + x] = scaleFactor * (x - (XRES / CELL)) / pow(distance, 3);
+			th_ptgravy[y * xblock2 + x] = scaleFactor * (y - (YRES / CELL)) / pow(distance, 3);
+		}
+	}
+	th_ptgravx[yblock2 * xblock2 / 2 + xblock2 / 2] = 0.0f;
+	th_ptgravy[yblock2 * xblock2 / 2 + xblock2 / 2] = 0.0f;
+
+	//transform point mass velocity maps
+	fftwf_execute(plan_ptgravx);
+	fftwf_execute(plan_ptgravy);
+	fftwf_destroy_plan(plan_ptgravx);
+	fftwf_destroy_plan(plan_ptgravy);
+	fftwf_free(th_ptgravx);
+	fftwf_free(th_ptgravy);
+
+	//clear padded gravmap
+	memset(th_gravmapbig, 0, xblock2 * yblock2 * sizeof(float));
+
+	grav_fft_status = true;
 }
 
-void Gravity::gravity_cleanup()
+void Gravity::grav_fft_cleanup()
 {
-	stop_grav_async();
-#ifdef GRAVFFT
-	grav_fft_cleanup();
-#endif
-	//Free gravity info
-	free(th_ogravmap);
-	free(th_gravmap);
-	free(th_gravy);
-	free(th_gravx);
-	free(th_gravp);
-	free(gravmap);
-	free(gravy);
-	free(gravx);
-	free(gravp);
-	free(gravmask);
-	free(obmap);
+	if (!grav_fft_status) return;
+	fftwf_free(th_ptgravxt);
+	fftwf_free(th_ptgravyt);
+	fftwf_free(th_gravmapbig);
+	fftwf_free(th_gravmapbigt);
+	fftwf_free(th_gravxbig);
+	fftwf_free(th_gravybig);
+	fftwf_free(th_gravxbigt);
+	fftwf_free(th_gravybigt);
+	fftwf_destroy_plan(plan_gravmap);
+	fftwf_destroy_plan(plan_gravx_inverse);
+	fftwf_destroy_plan(plan_gravy_inverse);
+	grav_fft_status = false;
 }
+#endif
 
 void Gravity::gravity_update_async()
 {
 	int result;
-	if (ngrav_enable)
-	{
-		bool signal_grav = false;
+	if (!enabled)
+		return;
+
+	bool signal_grav = false;
 
+	{
+		std::unique_lock<std::mutex> l(gravmutex, std::defer_lock);
+		if (l.try_lock())
 		{
-			std::unique_lock<std::mutex> l(gravmutex, std::defer_lock);
-			if (l.try_lock())
+			result = grav_ready;
+			if (result) //Did the gravity thread finish?
 			{
-				result = grav_ready;
-				if (result) //Did the gravity thread finish?
+				if (th_gravchanged && !ignoreNextResult)
 				{
-					float *tmpf;
-
-					if (th_gravchanged && !ignoreNextResult)
-					{
-					#if !defined(GRAVFFT) && defined(GRAV_DIFF)
-						memcpy(gravy, th_gravy, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-						memcpy(gravx, th_gravx, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-						memcpy(gravp, th_gravp, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-					#else
-						tmpf = gravy;
-						gravy = th_gravy;
-						th_gravy = tmpf;
-
-						tmpf = gravx;
-						gravx = th_gravx;
-						th_gravx = tmpf;
-
-						tmpf = gravp;
-						gravp = th_gravp;
-						th_gravp = tmpf;
-					#endif
-					}
-					ignoreNextResult = false;
+#if !defined(GRAVFFT) && defined(GRAV_DIFF)
+					memcpy(gravy, th_gravy, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+					memcpy(gravx, th_gravx, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+					memcpy(gravp, th_gravp, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+#else
+					std::swap(gravy, th_gravy);
+					std::swap(gravx, th_gravx);
+					std::swap(gravp, th_gravp);
+#endif
+				}
+				ignoreNextResult = false;
 
-					tmpf = gravmap;
-					gravmap = th_gravmap;
-					th_gravmap = tmpf;
+				std::swap(gravmap, th_gravmap);
 
-					grav_ready = 0; //Tell the other thread that we're ready for it to continue
-					signal_grav = true;
-				}
+				grav_ready = 0; //Tell the other thread that we're ready for it to continue
+				signal_grav = true;
 			}
 		}
+	}
 
-		if (signal_grav)
-		{
-			gravcv.notify_one();
-		}
-		//Apply the gravity mask
-		membwand(gravy, gravmask, (XRES/CELL)*(YRES/CELL)*sizeof(float), (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
-		membwand(gravx, gravmask, (XRES/CELL)*(YRES/CELL)*sizeof(float), (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
-		memset(gravmap, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+	if (signal_grav)
+	{
+		gravcv.notify_one();
 	}
+	unsigned int size = (XRES / CELL) * (YRES / CELL);
+	membwand(gravy, gravmask, size * sizeof(float), size * sizeof(unsigned));
+	membwand(gravx, gravmask, size * sizeof(float), size * sizeof(unsigned));
+	std::fill(&gravmap[0], &gravmap[size], 0);
 }
 
 void Gravity::update_grav_async()
 {
 	int done = 0;
 	int thread_done = 0;
-	memset(th_ogravmap, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(th_gravmap, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(th_gravy, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(th_gravx, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(th_gravp, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	//memset(th_gravy, 0, XRES*YRES*sizeof(float));
-	//memset(th_gravx, 0, XRES*YRES*sizeof(float));
-	//memset(th_gravp, 0, XRES*YRES*sizeof(float));
+	unsigned int size = (XRES / CELL) * (YRES / CELL);
+	std::fill(&th_ogravmap[0], &th_ogravmap[size], 0);
+	std::fill(&th_gravmap[0], &th_gravmap[size], 0);
+	std::fill(&th_gravy[0], &th_gravy[size], 0);
+	std::fill(&th_gravx[0], &th_gravx[size], 0);
+	std::fill(&th_gravp[0], &th_gravp[size], 0);
+
 #ifdef GRAVFFT
 	if (!grav_fft_status)
 		grav_fft_init();
@@ -170,7 +208,9 @@ void Gravity::update_grav_async()
 			done = 1;
 			grav_ready = 1;
 			thread_done = gravthread_done;
-		} else {
+		}
+		else
+		{
 			// wait for main thread
 			gravcv.wait(l);
 			done = grav_ready;
@@ -181,23 +221,24 @@ void Gravity::update_grav_async()
 
 void Gravity::start_grav_async()
 {
-	if (ngrav_enable)	//If it's already enabled, restart it
+	if (enabled)	//If it's already enabled, restart it
 		stop_grav_async();
 
 	gravthread_done = 0;
 	grav_ready = 0;
 	gravthread = std::thread([this]() { update_grav_async(); }); //Start asynchronous gravity simulation
-	ngrav_enable = 1;
+	enabled = true;
 
-	memset(gravy, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravx, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravp, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravmap, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+	unsigned int size = (XRES / CELL) * (YRES / CELL);
+	std::fill(&gravy[0], &gravy[size], 0);
+	std::fill(&gravx[0], &gravx[size], 0);
+	std::fill(&gravp[0], &gravp[size], 0);
+	std::fill(&gravmap[0], &gravmap[size], 0);
 }
 
 void Gravity::stop_grav_async()
 {
-	if (ngrav_enable)
+	if (enabled)
 	{
 		{
 			std::lock_guard<std::mutex> g(gravmutex);
@@ -205,119 +246,31 @@ void Gravity::stop_grav_async()
 		}
 		gravcv.notify_one();
 		gravthread.join();
-		ngrav_enable = 0;
+		enabled = false;
 	}
-	//Clear the grav velocities
-	memset(gravy, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravx, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravp, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memset(gravmap, 0, (XRES/CELL)*(YRES/CELL)*sizeof(float));
+	// Clear the grav velocities
+	unsigned int size = (XRES / CELL) * (YRES / CELL);
+	std::fill(&gravy[0], &gravy[size], 0);
+	std::fill(&gravx[0], &gravx[size], 0);
+	std::fill(&gravp[0], &gravp[size], 0);
+	std::fill(&gravmap[0], &gravmap[size], 0);
 }
 
 #ifdef GRAVFFT
-
-void Gravity::grav_fft_init()
-{
-	int xblock2 = XRES/CELL*2;
-	int yblock2 = YRES/CELL*2;
-	int x, y, fft_tsize = (xblock2/2+1)*yblock2;
-	float distance, scaleFactor;
-	fftwf_plan plan_ptgravx, plan_ptgravy;
-	if (grav_fft_status) return;
-
-	//use fftw malloc function to ensure arrays are aligned, to get better performance
-	th_ptgravx = (float*)fftwf_malloc(xblock2*yblock2*sizeof(float));
-	th_ptgravy = (float*)fftwf_malloc(xblock2*yblock2*sizeof(float));
-	th_ptgravxt = (fftwf_complex*)fftwf_malloc(fft_tsize*sizeof(fftwf_complex));
-	th_ptgravyt = (fftwf_complex*)fftwf_malloc(fft_tsize*sizeof(fftwf_complex));
-	th_gravmapbig = (float*)fftwf_malloc(xblock2*yblock2*sizeof(float));
-	th_gravmapbigt = (fftwf_complex*)fftwf_malloc(fft_tsize*sizeof(fftwf_complex));
-	th_gravxbig = (float*)fftwf_malloc(xblock2*yblock2*sizeof(float));
-	th_gravybig = (float*)fftwf_malloc(xblock2*yblock2*sizeof(float));
-	th_gravxbigt = (fftwf_complex*)fftwf_malloc(fft_tsize*sizeof(fftwf_complex));
-	th_gravybigt = (fftwf_complex*)fftwf_malloc(fft_tsize*sizeof(fftwf_complex));
-
-	//select best algorithm, could use FFTW_PATIENT or FFTW_EXHAUSTIVE but that increases the time taken to plan, and I don't see much increase in execution speed
-	plan_ptgravx = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravx, th_ptgravxt, FFTW_MEASURE);
-	plan_ptgravy = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_ptgravy, th_ptgravyt, FFTW_MEASURE);
-	plan_gravmap = fftwf_plan_dft_r2c_2d(yblock2, xblock2, th_gravmapbig, th_gravmapbigt, FFTW_MEASURE);
-	plan_gravx_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravxbigt, th_gravxbig, FFTW_MEASURE);
-	plan_gravy_inverse = fftwf_plan_dft_c2r_2d(yblock2, xblock2, th_gravybigt, th_gravybig, FFTW_MEASURE);
-
-	//(XRES/CELL)*(YRES/CELL)*4 is size of data array, scaling needed because FFTW calculates an unnormalized DFT
-	scaleFactor = -M_GRAV/((XRES/CELL)*(YRES/CELL)*4);
-	//calculate velocity map caused by a point mass
-	for (y=0; y<yblock2; y++)
-	{
-		for (x=0; x<xblock2; x++)
-		{
-			if (x==XRES/CELL && y==YRES/CELL) continue;
-			distance = sqrtf(pow(x-(XRES/CELL), 2.0f) + pow(y-(YRES/CELL), 2.0f));
-			th_ptgravx[y*xblock2+x] = scaleFactor*(x-(XRES/CELL)) / pow(distance, 3.0f);
-			th_ptgravy[y*xblock2+x] = scaleFactor*(y-(YRES/CELL)) / pow(distance, 3.0f);
-		}
-	}
-	th_ptgravx[yblock2*xblock2/2+xblock2/2] = 0.0f;
-	th_ptgravy[yblock2*xblock2/2+xblock2/2] = 0.0f;
-
-	//transform point mass velocity maps
-	fftwf_execute(plan_ptgravx);
-	fftwf_execute(plan_ptgravy);
-	fftwf_destroy_plan(plan_ptgravx);
-	fftwf_destroy_plan(plan_ptgravy);
-	fftwf_free(th_ptgravx);
-	fftwf_free(th_ptgravy);
-
-	//clear padded gravmap
-	memset(th_gravmapbig,0,xblock2*yblock2*sizeof(float));
-
-	grav_fft_status = true;
-}
-
-void Gravity::grav_fft_cleanup()
-{
-	if (!grav_fft_status) return;
-	fftwf_free(th_ptgravxt);
-	fftwf_free(th_ptgravyt);
-	fftwf_free(th_gravmapbig);
-	fftwf_free(th_gravmapbigt);
-	fftwf_free(th_gravxbig);
-	fftwf_free(th_gravybig);
-	fftwf_free(th_gravxbigt);
-	fftwf_free(th_gravybigt);
-	fftwf_destroy_plan(plan_gravmap);
-	fftwf_destroy_plan(plan_gravx_inverse);
-	fftwf_destroy_plan(plan_gravy_inverse);
-	grav_fft_status = false;
-}
-
 void Gravity::update_grav()
 {
-	int x, y, changed = 0;
 	int xblock2 = XRES/CELL*2, yblock2 = YRES/CELL*2;
-	int i, fft_tsize = (xblock2/2+1)*yblock2;
+	int fft_tsize = (xblock2/2+1)*yblock2;
 	float mr, mc, pr, pc, gr, gc;
-	for (y=0; y<YRES/CELL; y++)
-	{
-		if(changed)
-			break;
-		for (x=0; x<XRES/CELL; x++)
-		{
-			if(th_ogravmap[y*(XRES/CELL)+x] != th_gravmap[y*(XRES/CELL)+x] || bmap[y][x] != obmap[y][x]){
-				changed = 1;
-				break;
-			}
-		}
-	}
-	if(changed)
+	if (memcmp(th_ogravmap, th_gravmap, sizeof(float)*(XRES/CELL)*(YRES/CELL)) != 0)
 	{
 		th_gravchanged = 1;
 
 		membwand(th_gravmap, gravmask, (XRES/CELL)*(YRES/CELL)*sizeof(float), (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
 		//copy gravmap into padded gravmap array
-		for (y=0; y<YRES/CELL; y++)
+		for (int y = 0; y < YRES / CELL; y++)
 		{
-			for (x=0; x<XRES/CELL; x++)
+			for (int x = 0; x < XRES / CELL; x++)
 			{
 				th_gravmapbig[(y+YRES/CELL)*xblock2+XRES/CELL+x] = th_gravmap[y*(XRES/CELL)+x];
 			}
@@ -325,7 +278,7 @@ void Gravity::update_grav()
 		//transform gravmap
 		fftwf_execute(plan_gravmap);
 		//do convolution (multiply the complex numbers)
-		for (i=0; i<fft_tsize; i++)
+		for (int i = 0; i < fft_tsize; i++)
 		{
 			mr = th_gravmapbigt[i][0];
 			mc = th_gravmapbigt[i][1];
@@ -345,9 +298,9 @@ void Gravity::update_grav()
 		//inverse transform, and copy from padded arrays into normal velocity maps
 		fftwf_execute(plan_gravx_inverse);
 		fftwf_execute(plan_gravy_inverse);
-		for (y=0; y<YRES/CELL; y++)
+		for (int y = 0; y < YRES / CELL; y++)
 		{
-			for (x=0; x<XRES/CELL; x++)
+			for (int x = 0; x < XRES / CELL; x++)
 			{
 				th_gravx[y*(XRES/CELL)+x] = th_gravxbig[y*xblock2+x];
 				th_gravy[y*(XRES/CELL)+x] = th_gravybig[y*xblock2+x];
@@ -359,8 +312,9 @@ void Gravity::update_grav()
 	{
 		th_gravchanged = 0;
 	}
-	memcpy(th_ogravmap, th_gravmap, (XRES/CELL)*(YRES/CELL)*sizeof(float));
-	memcpy(obmap, bmap, (XRES/CELL)*(YRES/CELL)*sizeof(unsigned char));
+
+	// Copy th_ogravmap into th_gravmap (doesn't matter what th_ogravmap is afterwards)
+	std::swap(th_gravmap, th_ogravmap);
 }
 
 #else
@@ -427,121 +381,147 @@ void Gravity::update_grav(void)
 
 
 
-void Gravity::grav_mask_r(int x, int y, char checkmap[YRES/CELL][XRES/CELL], char shape[YRES/CELL][XRES/CELL], char *shapeout)
+bool Gravity::grav_mask_r(int x, int y, char checkmap[YRES/CELL][XRES/CELL], char shape[YRES/CELL][XRES/CELL])
 {
-	if(x < 0 || x >= XRES/CELL || y < 0 || y >= YRES/CELL)
-		return;
-	if(x == 0 || y ==0 || y == (YRES/CELL)-1 || x == (XRES/CELL)-1)
-		*shapeout = 1;
-
-	int x1 = x, x2 = x;
-	while (x1 >= 1)
+	int x1, x2;
+	bool ret = false;
+	try
 	{
-		if(checkmap[y][x1-1] || bmap[y][x1-1]==WL_GRAV)
-			break;
-		x1--;
+		CoordStack cs;
+		cs.push(x, y);
+		do
+		{
+			cs.pop(x, y);
+			x1 = x2 = x;
+			while (x1 >= 0)
+			{
+				if (x1 == 0)
+				{
+					ret = true;
+					break;
+				}
+				else if (checkmap[y][x1-1] || bmap[y][x1-1] == WL_GRAV)
+					break;
+				x1--;
+			}
+			while (x2 <= XRES/CELL-1)
+			{
+				if (x2 == XRES/CELL-1)
+				{
+					ret = true;
+					break;
+				}
+				else if (checkmap[y][x2+1] || bmap[y][x2+1] == WL_GRAV)
+					break;
+				x2++;
+			}
+			for (x = x1; x <= x2; x++)
+			{
+				shape[y][x] = 1;
+				checkmap[y][x] = 1;
+			}
+			if (y == 0)
+			{
+				for (x = x1; x <= x2; x++)
+					if (bmap[y][x] != WL_GRAV)
+						ret = true;
+			}
+			else if (y >= 1)
+			{
+				for (x = x1; x <= x2; x++)
+					if (!checkmap[y-1][x] && bmap[y-1][x] != WL_GRAV)
+					{
+						if (y-1 == 0)
+							ret = true;
+						cs.push(x, y-1);
+					}
+			}
+			if (y < YRES/CELL-1)
+				for (x=x1; x<=x2; x++)
+					if (!checkmap[y+1][x] && bmap[y+1][x] != WL_GRAV)
+					{
+						if (y+1 == YRES/CELL-1)
+							ret = true;
+						cs.push(x, y+1);
+					}
+		} while (cs.getSize()>0);
 	}
-	while (x2 < (XRES/CELL)-1)
+	catch (std::exception& e)
 	{
-		if(checkmap[y][x2+1] || bmap[y][x2+1]==WL_GRAV)
-			break;
-		x2++;
+		std::cerr << e.what() << std::endl;
+		ret = false;
 	}
-
-	// fill span
-	for (x = x1; x <= x2; x++)
-		checkmap[y][x] = shape[y][x] = 1;
-
-	if(y >= 1)
-		for(x = x1; x <= x2; x++)
-			if(!checkmap[y-1][x] && bmap[y-1][x]!=WL_GRAV)
-				grav_mask_r(x, y-1, checkmap, shape, shapeout);
-	if(y < (YRES/CELL)-1)
-		for(x = x1; x <= x2; x++)
-			if(!checkmap[y+1][x] && bmap[y+1][x]!=WL_GRAV)
-				grav_mask_r(x, y+1, checkmap, shape, shapeout);
-	return;
+	return ret;
 }
-void Gravity::mask_free(mask_el *c_mask_el){
-	if(c_mask_el==NULL)
+void Gravity::mask_free(mask_el *c_mask_el)
+{
+	if (c_mask_el == nullptr)
 		return;
-	if(c_mask_el->next!=NULL)
-		mask_free((mask_el*)c_mask_el->next);
-	free(c_mask_el->shape);
-	free(c_mask_el);
+	delete[] c_mask_el->next;
+	delete[] c_mask_el->shape;
+	delete[] c_mask_el;
 }
+
 void Gravity::gravity_mask()
 {
 	char checkmap[YRES/CELL][XRES/CELL];
-	int x = 0, y = 0;
 	unsigned maskvalue;
-	mask_el *t_mask_el = NULL;
-	mask_el *c_mask_el = NULL;
-	if(!gravmask)
+	mask_el *t_mask_el = nullptr;
+	mask_el *c_mask_el = nullptr;
+	if (!gravmask)
 		return;
 	memset(checkmap, 0, sizeof(checkmap));
-	for(x = 0; x < XRES/CELL; x++)
+	for (int x = 0; x < XRES / CELL; x++)
 	{
-		for(y = 0; y < YRES/CELL; y++)
+		for(int y = 0; y < YRES / CELL; y++)
 		{
-			if(bmap[y][x]!=WL_GRAV && checkmap[y][x] == 0)
+			if (bmap[y][x] != WL_GRAV && checkmap[y][x] == 0)
 			{
-				//Create a new shape
-				if(t_mask_el==NULL){
-					t_mask_el = (mask_el *)malloc(sizeof(mask_el));
-					t_mask_el->shape = (char *)malloc((XRES/CELL)*(YRES/CELL));
-					memset(t_mask_el->shape, 0, (XRES/CELL)*(YRES/CELL));
+				// Create a new shape
+				if (t_mask_el == nullptr)
+				{
+					t_mask_el = new mask_el[sizeof(mask_el)];
+					t_mask_el->shape = new char[(XRES / CELL) * (YRES / CELL)];
+					std::fill(&t_mask_el->shape[0], &t_mask_el->shape[(XRES / CELL) * (YRES / CELL)], 0);
 					t_mask_el->shapeout = 0;
-					t_mask_el->next = NULL;
+					t_mask_el->next = nullptr;
 					c_mask_el = t_mask_el;
-				} else {
-					c_mask_el->next = (mask_el *)malloc(sizeof(mask_el));
-					c_mask_el = (mask_el *)c_mask_el->next;
-					c_mask_el->shape = (char *)malloc((XRES/CELL)*(YRES/CELL));
-					memset(c_mask_el->shape, 0, (XRES/CELL)*(YRES/CELL));
+				}
+				else
+				{
+					c_mask_el->next = new mask_el[sizeof(mask_el)];
+					c_mask_el = c_mask_el->next;
+					c_mask_el->shape = new char[(XRES / CELL) * (YRES / CELL)];
+					std::fill(&c_mask_el->shape[0], &c_mask_el->shape[(XRES / CELL) * (YRES / CELL)], 0);
 					c_mask_el->shapeout = 0;
-					c_mask_el->next = NULL;
+					c_mask_el->next = nullptr;
 				}
-				//Fill the shape
-				grav_mask_r(x, y, (char (*)[XRES/CELL])checkmap, (char (*)[XRES/CELL])c_mask_el->shape, (char*)&c_mask_el->shapeout);
+				// Fill the shape
+				if (grav_mask_r(x, y, checkmap, reinterpret_cast<char(*)[XRES/CELL]>(c_mask_el->shape)))
+					c_mask_el->shapeout = 1;
 			}
 		}
 	}
 	c_mask_el = t_mask_el;
-	memset(gravmask, 0, (XRES/CELL)*(YRES/CELL)*sizeof(unsigned));
-	while(c_mask_el!=NULL)
+	std::fill(&gravmask[0], &gravmask[(XRES / CELL) * (YRES / CELL)], 0);
+	while (c_mask_el != nullptr)
 	{
 		char *cshape = c_mask_el->shape;
-		for(x = 0; x < XRES/CELL; x++)
+		for (int x = 0; x < XRES / CELL; x++)
 		{
-			for(y = 0; y < YRES/CELL; y++)
+			for (int y = 0; y < YRES / CELL; y++)
 			{
-				if(cshape[y*(XRES/CELL)+x]){
-					if(c_mask_el->shapeout)
+				if (cshape[y * (XRES / CELL) + x])
+				{
+					if (c_mask_el->shapeout)
 						maskvalue = 0xFFFFFFFF;
 					else
 						maskvalue = 0x00000000;
-					gravmask[y*(XRES/CELL)+x] = maskvalue;
+					gravmask[y * (XRES / CELL) + x] = maskvalue;
 				}
 			}
 		}
-		c_mask_el = (mask_el*)c_mask_el->next;
+		c_mask_el = c_mask_el->next;
 	}
 	mask_free(t_mask_el);
 }
-#ifdef GRAVFFT
-Gravity::Gravity():
-	grav_fft_status(false)
-{
-	gravity_init();
-}
-#else
-Gravity::Gravity()
-{
-	gravity_init();
-}
-#endif
-Gravity::~Gravity()
-{
-	gravity_cleanup();
-}
diff --git a/src/simulation/Gravity.h b/src/simulation/Gravity.h
index 312b875856..6ef35dd2cc 100644
--- a/src/simulation/Gravity.h
+++ b/src/simulation/Gravity.h
@@ -12,90 +12,81 @@
 
 class Simulation;
 
-struct mask_el {
-	char *shape;
-	char shapeout;
-	void *next;
-};
-typedef struct mask_el mask_el;
-
 class Gravity
 {
 private:
 
-	float *th_ogravmap;
-	float *th_gravmap;
-	float *th_gravx;
-	float *th_gravy;
-	float *th_gravp;
+	bool enabled = false;
 
-	int th_gravchanged;
+	// Maps to be processed by the gravity thread
+	float *th_ogravmap = nullptr;
+	float *th_gravmap = nullptr;
+	float *th_gravx = nullptr;
+	float *th_gravy = nullptr;
+	float *th_gravp = nullptr;
+
+	int th_gravchanged = 0;
 
 	std::thread gravthread;
 	std::mutex gravmutex;
 	std::condition_variable gravcv;
-	int grav_ready;
-	int gravthread_done;
+	int grav_ready = 0;
+	int gravthread_done = 0;
+	bool ignoreNextResult = false;
 
 #ifdef GRAVFFT
-	bool grav_fft_status;
-	float *th_ptgravx, *th_ptgravy, *th_gravmapbig, *th_gravxbig, *th_gravybig;
+	bool grav_fft_status = false;
+	float *th_ptgravx = nullptr;
+	float *th_ptgravy = nullptr;
+	float *th_gravmapbig = nullptr;
+	float *th_gravxbig = nullptr;
+	float *th_gravybig = nullptr;
+
 	fftwf_complex *th_ptgravxt, *th_ptgravyt, *th_gravmapbigt, *th_gravxbigt, *th_gravybigt;
 	fftwf_plan plan_gravmap, plan_gravx_inverse, plan_gravy_inverse;
 #endif
 
-	//Simulation * sim;
+	struct mask_el {
+		char *shape;
+		char shapeout;
+		mask_el *next;
+	};
+	using mask_el = struct mask_el;
+
+	bool grav_mask_r(int x, int y, char checkmap[YRES/CELL][XRES/CELL], char shape[YRES/CELL][XRES/CELL]);
+	void mask_free(mask_el *c_mask_el);
+
+	void update_grav();
+	void update_grav_async();
+
+
+#ifdef GRAVFFT
+	void grav_fft_init();
+	void grav_fft_cleanup();
+#endif
+
 public:
-	unsigned *gravmask;
-	float *gravmap;
-	float *gravp;
-	float *gravy;
-	float *gravx;
+	//Maps to be used by the main thread
+	float *gravmap = nullptr;
+	float *gravp = nullptr;
+	float *gravy = nullptr;
+	float *gravx = nullptr;
+	unsigned *gravmask = nullptr;
+
 	unsigned char (*bmap)[XRES/CELL];
-	unsigned char (*obmap)[XRES/CELL];
-	int ngrav_enable;
-	void grav_mask_r(int x, int y, char checkmap[YRES/CELL][XRES/CELL], char shape[YRES/CELL][XRES/CELL], char *shapeout);
-	void mask_free(mask_el *c_mask_el);
+
+	bool IsEnabled() { return enabled; }
 
 	void Clear();
 
-	void gravity_init();
-	void gravity_cleanup();
 	void gravity_update_async();
 
-	void update_grav_async();
-
 	void start_grav_async();
 	void stop_grav_async();
-	void update_grav();
 	void gravity_mask();
 
-	void bilinear_interpolation(float *src, float *dst, int sw, int sh, int rw, int rh);
-
-	#ifdef GRAVFFT
-	void grav_fft_init();
-	void grav_fft_cleanup();
-	#endif
-
 	Gravity();
 	~Gravity();
 };
 
-/*extern int ngrav_enable; //Newtonian gravity
-extern int gravwl_timeout;
-extern int gravityMode;*/
-
-/*float *gravmap;//Maps to be used by the main thread
-float *gravp;
-float *gravy;
-float *gravx;
-unsigned *gravmask;
-
-float *th_ogravmap;// Maps to be processed by the gravity thread
-float *th_gravmap;
-float *th_gravx;
-float *th_gravy;
-float *th_gravp;*/
-
-
 #endif
diff --git a/src/simulation/Simulation.cpp b/src/simulation/Simulation.cpp
index 90509c7491..0639564716 100644
--- a/src/simulation/Simulation.cpp
+++ b/src/simulation/Simulation.cpp
@@ -492,7 +492,7 @@ void Simulation::SaveSimOptions(GameSave * gameSave)
 	gameSave->edgeMode = edgeMode;
 	gameSave->legacyEnable = legacy_enable;
 	gameSave->waterEEnabled = water_equal_test;
-	gameSave->gravityEnable = grav->ngrav_enable;
+	gameSave->gravityEnable = grav->IsEnabled();
 	gameSave->aheatEnable = aheat_enable;
 }
 
@@ -538,7 +538,7 @@ void Simulation::Restore(const Snapshot & snap)
 	RecalcFreeParticles(false);
 	std::copy(snap.PortalParticles.begin(), snap.PortalParticles.end(), &portalp[0][0][0]);
 	std::copy(snap.WirelessData.begin(), snap.WirelessData.end(), &wireless[0][0]);
-	if (grav->ngrav_enable)
+	if (grav->IsEnabled())
 	{
 		grav->Clear();
 		std::copy(snap.GravVelocityX.begin(), snap.GravVelocityX.end(), gravx);
@@ -702,7 +702,7 @@ SimulationSample Simulation::GetSample(int x, int y)
 		sample.AirVelocityX = vx[y/CELL][x/CELL];
 		sample.AirVelocityY = vy[y/CELL][x/CELL];
 
-		if(grav->ngrav_enable)
+		if(grav->IsEnabled())
 		{
 			sample.Gravity = gravp[(y/CELL)*(XRES/CELL)+(x/CELL)];
 			sample.GravityVelocityX = gravx[(y/CELL)*(XRES/CELL)+(x/CELL)];
@@ -4702,7 +4702,7 @@ void Simulation::UpdateParticles(int start, int end)
 								goto movedone;
 							}
 						}
-						if (elements[t].Falldown>1 && !grav->ngrav_enable && gravityMode==0 && parts[i].vy>fabsf(parts[i].vx))
+						if (elements[t].Falldown>1 && !grav->IsEnabled() && gravityMode==0 && parts[i].vy>fabsf(parts[i].vx))
 						{
 							s = 0;
 							// stagnant is true if FLAG_STAGNANT was set for this particle in previous frame
@@ -5209,7 +5209,7 @@ void Simulation::BeforeSim()
 		if(aheat_enable)
 			air->update_airh();
 
-		if(grav->ngrav_enable)
+		if(grav->IsEnabled())
 		{
 			grav->gravity_update_async();