Sculpt: Grid based PBVH

* PBVH can now be created contain both from face grids or standard
  meshes. The former is much quicker to build for high res meshes.
* Moved some drawing code into pbvh (mostly for the frustum test).
* Moved ray intersection code into pbvh.
* GPU buffers also can be built from either mesh or grids now.
* Updated sculpt code to work with this. The ugly part is that there
  is now a macro for iterating over vertices, to handle both cases,
  and some duplicated code for e.g. undo.
* Smooth brush does not work yet with grids.

7 files changed, 969 insertions, 501 deletions

diff --git a/source/blender/blenkernel/intern/cdderivedmesh.c b/source/blender/blenkernel/intern/cdderivedmesh.c
index e64d8a24934..3007564e333 100644
--- a/source/blender/blenkernel/intern/cdderivedmesh.c
+++ b/source/blender/blenkernel/intern/cdderivedmesh.c
@@ -197,7 +197,7 @@ static struct PBVH *cdDM_getPBVH(Object *ob, DerivedMesh *dm)
 		Mesh *me= ob->data;
 
 		cddm->pbvh = BLI_pbvh_new();
-		BLI_pbvh_build(cddm->pbvh, me->mface, me->mvert,
+		BLI_pbvh_build_mesh(cddm->pbvh, me->mface, me->mvert,
 			       me->totface, me->totvert);
 	}
 
@@ -393,60 +393,6 @@ static void cdDM_drawLooseEdges(DerivedMesh *dm)
 	}
 }
 
-static int nodes_drawn = 0;
-static int is_partial = 0;
-/* XXX: Just a temporary replacement for the real drawing code */
-static void draw_partial_cb(PBVHNode *node, void *data)
-{
-	/* XXX: Just some quick code to show leaf nodes in different colors */
-	/*float col[3]; int i;
-	if(is_partial) {
-		col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
-	}
-	else {
-		srand((long long)data_v);
-		for(i = 0; i < 3; ++i)
-			col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
-	}
-	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
-
-	glColor3f(1, 0, 0);*/
-	GPU_draw_buffers(BLI_pbvh_node_get_draw_buffers(node));
-	++nodes_drawn;
-}
-
-/* Adapted from:
-   http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
-   Returns true if the AABB is at least partially within the frustum
-   (ok, not a real frustum), false otherwise.
-*/
-int planes_contain_AABB(PBVHNode *node, void *data)
-{
-	float (*planes)[4] = data;
-	int i, axis;
-	float vmin[3], vmax[3], bb_min[3], bb_max[3];
-
-	BLI_pbvh_node_get_BB(node, bb_min, bb_max);
-
-	for(i = 0; i < 4; ++i) { 
-		for(axis = 0; axis < 3; ++axis) {
-			if(planes[i][axis] > 0) { 
-				vmin[axis] = bb_min[axis];
-				vmax[axis] = bb_max[axis];
-			}
-			else {
-				vmin[axis] = bb_max[axis];
-				vmax[axis] = bb_min[axis];
-			}
-		}
-		
-		if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
-			return 0;
-	} 
-
-	return 1;
-}
-
 static void cdDM_drawFacesSolid(DerivedMesh *dm,
 				float (*partial_redraw_planes)[4],
 				int (*setMaterial)(int, void *attribs))
@@ -468,26 +414,11 @@ static void cdDM_drawFacesSolid(DerivedMesh *dm,
 	if(cddm->pbvh) {
 		float (*face_nors)[3] = CustomData_get_layer(&dm->faceData, CD_NORMAL);
 
-		BLI_pbvh_update(cddm->pbvh, PBVH_UpdateNormals|PBVH_UpdateDrawBuffers,
-			face_nors);
-
 		/* should be per face */
 		if(dm->numFaceData && mface->flag & ME_SMOOTH)
 			glShadeModel(GL_SMOOTH);
 
-		if(partial_redraw_planes) {
-			BLI_pbvh_search_callback(cddm->pbvh, planes_contain_AABB,
-					partial_redraw_planes, draw_partial_cb, NULL);
-		}
-		else {
-			BLI_pbvh_search_callback(cddm->pbvh, NULL, NULL,
-					draw_partial_cb, NULL);
-		}
-
-		is_partial = !!partial_redraw_planes;
-
-		//printf("nodes drawn=%d\n", nodes_drawn);
-		nodes_drawn = 0;
+		BLI_pbvh_draw(cddm->pbvh, partial_redraw_planes, face_nors);
 
 		glShadeModel(GL_FLAT);
 
diff --git a/source/blender/blenkernel/intern/subsurf_ccg.c b/source/blender/blenkernel/intern/subsurf_ccg.c
index e465d17f498..30766931a0b 100644
--- a/source/blender/blenkernel/intern/subsurf_ccg.c
+++ b/source/blender/blenkernel/intern/subsurf_ccg.c
@@ -819,6 +819,7 @@ static void ccgDM_copyFinalVertArray(DerivedMesh *dm, MVert *mvert)
 		for(x = 1; x < edgeSize - 1; x++, i++) {
 			vd= ccgSubSurf_getEdgeData(ss, e, x);
 			copy_v3_v3(mvert[i].co, vd->co);
+			/* TODO CCGSubsurf does not set these */
 			normal_float_to_short_v3(mvert[i].no, vd->no);
 		}
 	}
@@ -1230,8 +1231,7 @@ static void ccgDM_drawFacesSolid(DerivedMesh *dm, float (*partial_redraw_planes)
 	char *faceFlags = ccgdm->faceFlags;
 	int step = 1; //(fast)? gridSize-1: 1;
 
-#if 0
-	if(ccgdm->pbvh && ccgdm->multires.mmd && !fast) {
+	if(ccgdm->pbvh && ccgdm->multires.mmd) { // && !fast) {
 		CCGFace **faces;
 		int totface;
 
@@ -1252,7 +1252,6 @@ static void ccgDM_drawFacesSolid(DerivedMesh *dm, float (*partial_redraw_planes)
 
 		return;
 	}
-#endif
 
 	fi = ccgSubSurf_getFaceIterator(ss);
 	for (; !ccgFaceIterator_isStopped(fi); ccgFaceIterator_next(fi)) {
@@ -2135,12 +2134,12 @@ static DMGridAdjacency *ccgDM_getGridAdjacency(DerivedMesh *dm)
 static struct PBVH *ccgDM_getPBVH(Object *ob, DerivedMesh *dm)
 {
 	CCGDerivedMesh *ccgdm= (CCGDerivedMesh*)dm;
-	//int gridSize, numGrids;
+	int gridSize, numGrids;
 
 	if(ccgdm->pbvh)
 		return ccgdm->pbvh;
 
-	/*if(ccgdm->multires.mmd) {
+	if(ccgdm->multires.mmd) {
 		ccgdm_create_grids(dm);
 
 		gridSize = ccgDM_getGridSize(dm);
@@ -2150,11 +2149,11 @@ static struct PBVH *ccgDM_getPBVH(Object *ob, DerivedMesh *dm)
 		BLI_pbvh_build_grids(ccgdm->pbvh, ccgdm->gridData, numGrids, gridSize,
 			(void**)ccgdm->gridFaces);
 	}
-	else*/ if(ob->type == OB_MESH) {
+	else if(ob->type == OB_MESH) {
 		Mesh *me= ob->data;
 
 		ccgdm->pbvh = BLI_pbvh_new();
-		BLI_pbvh_build(ccgdm->pbvh, me->mface, me->mvert,
+		BLI_pbvh_build_mesh(ccgdm->pbvh, me->mface, me->mvert,
 			       me->totface, me->totvert);
 	}
 
diff --git a/source/blender/blenlib/BLI_pbvh.h b/source/blender/blenlib/BLI_pbvh.h
index 360a9937498..5c5277dc091 100644
--- a/source/blender/blenlib/BLI_pbvh.h
+++ b/source/blender/blenlib/BLI_pbvh.h
@@ -27,6 +27,7 @@
 
 struct MFace;
 struct MVert;
+struct DMGridData;
 struct PBVH;
 struct PBVHNode;
 struct ListBase;
@@ -44,12 +45,12 @@ typedef void (*BLI_pbvh_HitCallback)(PBVHNode *node, void *data);
 /* Building */
 
 PBVH *BLI_pbvh_new(void);
-void BLI_pbvh_build(PBVH *bvh, struct MFace *faces, struct MVert *verts,
+void BLI_pbvh_build_mesh(PBVH *bvh, struct MFace *faces, struct MVert *verts,
 		    int totface, int totvert);
+void BLI_pbvh_build_grids(PBVH *bvh, struct DMGridData **grids, int totgrid,
+	int gridsize, void **gridfaces);
 void BLI_pbvh_free(PBVH *bvh);
 
-void BLI_pbvh_set_source(PBVH *bvh, struct MVert *, struct MFace *mface);
-
 /* Hierarchical Search in the BVH, two methods:
    * for each hit calling a callback
    * gather nodes in an array (easy to multithread) */
@@ -69,6 +70,14 @@ void BLI_pbvh_search_gather(PBVH *bvh,
 
 void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitCallback cb, void *data,
 		      float ray_start[3], float ray_normal[3], int original);
+int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
+	float ray_start[3], float ray_normal[3], float *dist);
+
+/* Drawing */
+
+void BLI_pbvh_node_draw(PBVHNode *node, void *data);
+int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data);
+void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3]);
 
 /* Node Access */
 
@@ -84,11 +93,11 @@ typedef enum {
 
 void BLI_pbvh_node_mark_update(PBVHNode *node);
 
-void BLI_pbvh_node_get_verts(PBVHNode *node, int **vert_indices,
-	int *totvert, int *allverts);
-void BLI_pbvh_node_get_faces(PBVHNode *node, int **face_indices,
-	int **face_vert_indices, int *totface);
-void *BLI_pbvh_node_get_draw_buffers(PBVHNode *node);
+void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node,
+	int **grid_indices, int *totgrid, int *maxgrid, int *gridsize);
+void BLI_pbvh_node_num_verts(PBVH *bvh, PBVHNode *node,
+	int *uniquevert, int *totvert);
+
 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3]);
 void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max[3]);
 
@@ -96,6 +105,91 @@ void BLI_pbvh_node_get_original_BB(PBVHNode *node, float bb_min[3], float bb_max
 
 void BLI_pbvh_update(PBVH *bvh, int flags, float (*face_nors)[3]);
 void BLI_pbvh_redraw_BB(PBVH *bvh, float bb_min[3], float bb_max[3]);
+void BLI_pbvh_get_grid_updates(PBVH *bvh, void ***gridfaces, int *totface);
+
+/* Vertex Iterator */
+
+/* this iterator has quite a lot of code, but it's designed to:
+   - allow the compiler to eliminate dead code and variables
+   - spend most of the time in the relatively simple inner loop */
+
+#define PBVH_ITER_ALL		0
+#define PBVH_ITER_UNIQUE	1
+
+typedef struct PBVHVertexIter {
+	/* iteration */
+	int g;
+	int width;
+	int height;
+	int skip;
+	int gx;
+	int gy;
+	int i;
+
+	/* grid */
+	struct DMGridData **grids;
+	struct DMGridData *grid;
+	int *grid_indices;
+	int totgrid;
+	int gridsize;
+
+	/* mesh */
+	struct MVert *mverts;
+	int totvert;
+	int *vert_indices;
+
+	/* result: these are all computed in the macro, but we assume
+	   that compiler optimizations will skip the ones we don't use */
+	struct MVert *mvert;
+	float *co;
+	short *no;
+	float *fno;
+} PBVHVertexIter;
+
+void BLI_pbvh_node_verts_iter_init(PBVH *bvh, PBVHNode *node, PBVHVertexIter *vi, int mode);
+
+#define BLI_pbvh_vertex_iter_begin(bvh, node, vi, mode) \
+	/* XXX breaks aliasing! */ \
+	BLI_pbvh_node_verts_iter_init(bvh, node, &vi, mode); \
+	\
+	for(vi.i=0, vi.g=0; vi.g<vi.totgrid; vi.g++) { \
+		if(vi.grids) { \
+			vi.width= vi.gridsize; \
+			vi.height= vi.gridsize; \
+			vi.grid= vi.grids[vi.grid_indices[vi.g]]; \
+			vi.skip= 0; \
+		 	\
+			/*if(mode == PVBH_ITER_UNIQUE) { \
+				vi.grid += subm->grid.offset; \
+				vi.skip= subm->grid.skip; \
+				vi.grid -= skip; \
+			}*/ \
+		} \
+		else { \
+			vi.width= vi.totvert; \
+			vi.height= 1; \
+		} \
+	 	\
+		for(vi.gy=0; vi.gy<vi.height; vi.gy++) { \
+			if(vi.grid) vi.grid += vi.skip; \
+			\
+			for(vi.gx=0; vi.gx<vi.width; vi.gx++, vi.i++) { \
+				if(vi.grid) { \
+					vi.co= vi.grid->co; \
+					vi.fno= vi.grid->no; \
+					vi.grid++; \
+				} \
+				else { \
+					vi.mvert= &vi.mverts[vi.vert_indices[vi.gx]]; \
+					vi.co= vi.mvert->co; \
+					vi.no= vi.mvert->no; \
+				} \
+
+#define BLI_pbvh_vertex_iter_end \
+			} \
+		} \
+	}
+
 
 #endif /* BLI_PBVH_H */
 
diff --git a/source/blender/blenlib/intern/pbvh.c b/source/blender/blenlib/intern/pbvh.c
index c2f0705d8c2..023db54eb1c 100644
--- a/source/blender/blenlib/intern/pbvh.c
+++ b/source/blender/blenlib/intern/pbvh.c
@@ -32,6 +32,7 @@
 #include "BLI_ghash.h"
 #include "BLI_pbvh.h"
 
+#include "BKE_DerivedMesh.h"
 #include "BKE_mesh.h"
 #include "BKE_utildefines.h"
 
@@ -87,12 +88,12 @@ struct PBVHNode {
 	/* For internal nodes */
 	int children_offset;
 
-	/* Pointer into bvh face_indices */
-	int *face_indices;
+	/* Pointer into bvh prim_indices */
+	int *prim_indices;
 	int *face_vert_indices;
 
-	unsigned short totface;
-	unsigned short uniq_verts, face_verts;
+	unsigned int totprim;
+	unsigned int uniq_verts, face_verts;
 
 	char flag;
 };
@@ -101,14 +102,22 @@ struct PBVH {
 	PBVHNode *nodes;
 	int node_mem_count, totnode;
 
-	int *face_indices;
-	int totface;
+	int *prim_indices;
+	int totprim;
 	int totvert;
 
+	int leaf_limit;
+
 	/* Mesh data */
 	MVert *verts;
 	MFace *faces;
 
+	/* Grid Data */
+	DMGridData **grids;
+	void **gridfaces;
+	int totgrid;
+	int gridsize;
+
 	/* Only used during BVH build and update,
 	   don't need to remain valid after */
 	BLI_bitmap vert_bitmap;
@@ -201,12 +210,12 @@ static void update_node_vb(PBVH *bvh, PBVHNode *node)
 	BB_reset(&vb);
 	
 	if(node->flag & PBVH_Leaf) {
-		int i, totvert= node->uniq_verts + node->face_verts;
+		PBVHVertexIter vd;
 
-		for(i = 0; i < totvert; ++i) {
-			float *co= bvh->verts[node->vert_indices[i]].co;
-			BB_expand(&vb, co);
+		BLI_pbvh_vertex_iter_begin(bvh, node, vd, PBVH_ITER_ALL) {
+			BB_expand(&vb, vd.co);
 		}
+		BLI_pbvh_vertex_iter_end;
 	}
 	else {
 		BB_expand_with_bb(&vb,
@@ -220,28 +229,28 @@ static void update_node_vb(PBVH *bvh, PBVHNode *node)
 
 /* Adapted from BLI_kdopbvh.c */
 /* Returns the index of the first element on the right of the partition */
-static int partition_indices(int *face_indices, int lo, int hi, int axis,
+static int partition_indices(int *prim_indices, int lo, int hi, int axis,
 			     float mid, BBC *prim_bbc)
 {
 	int i=lo, j=hi;
 	for(;;) {
-		for(; prim_bbc[face_indices[i]].bcentroid[axis] < mid; i++);
-		for(; mid < prim_bbc[face_indices[j]].bcentroid[axis]; j--);
+		for(; prim_bbc[prim_indices[i]].bcentroid[axis] < mid; i++);
+		for(; mid < prim_bbc[prim_indices[j]].bcentroid[axis]; j--);
 		
 		if(!(i < j))
 			return i;
 		
-		SWAP(int, face_indices[i], face_indices[j]);
+		SWAP(int, prim_indices[i], prim_indices[j]);
 		i++;
 	}
 }
 
-void check_partitioning(int *face_indices, int lo, int hi, int axis,
+void check_partitioning(int *prim_indices, int lo, int hi, int axis,
 			       float mid, BBC *prim_bbc, int index_of_2nd_partition)
 {
 	int i;
 	for(i = lo; i <= hi; ++i) {
-		const float c = prim_bbc[face_indices[i]].bcentroid[axis];
+		const float c = prim_bbc[prim_indices[i]].bcentroid[axis];
 
 		if((i < index_of_2nd_partition && c > mid) ||
 		   (i > index_of_2nd_partition && c < mid)) {
@@ -269,8 +278,8 @@ static void grow_nodes(PBVH *bvh, int totnode)
 /* Add a vertex to the map, with a positive value for unique vertices and
    a negative value for additional vertices */
 static int map_insert_vert(PBVH *bvh, GHash *map,
-			    unsigned short *face_verts,
-			    unsigned short *uniq_verts, int vertex)
+			    unsigned int *face_verts,
+			    unsigned int *uniq_verts, int vertex)
 {
 	void *value, *key = SET_INT_IN_POINTER(vertex);
 
@@ -293,7 +302,7 @@ static int map_insert_vert(PBVH *bvh, GHash *map,
 }
 
 /* Find vertices used by the faces in this node and update the draw buffers */
-static void build_leaf_node(PBVH *bvh, PBVHNode *node)
+static void build_mesh_leaf_node(PBVH *bvh, PBVHNode *node)
 {
 	GHashIterator *iter;
 	GHash *map;
@@ -302,13 +311,13 @@ static void build_leaf_node(PBVH *bvh, PBVHNode *node)
 	map = BLI_ghash_new(BLI_ghashutil_inthash, BLI_ghashutil_intcmp);
 	
 	node->uniq_verts = node->face_verts = 0;
-	totface= node->totface;
+	totface= node->totprim;
 
 	node->face_vert_indices = MEM_callocN(sizeof(int) *
 					 4*totface, "bvh node face vert indices");
 
 	for(i = 0; i < totface; ++i) {
-		MFace *f = bvh->faces + node->face_indices[i];
+		MFace *f = bvh->faces + node->prim_indices[i];
 		int sides = f->v4 ? 4 : 3;
 
 		for(j = 0; j < sides; ++j) {
@@ -341,15 +350,22 @@ static void build_leaf_node(PBVH *bvh, PBVHNode *node)
 			node->face_vert_indices[i]= -node->face_vert_indices[i] + node->uniq_verts - 1;
 
 	node->draw_buffers =
-		GPU_build_buffers(map, bvh->verts, bvh->faces,
-				  node->face_indices,
-				  node->totface, node->vert_indices,
+		GPU_build_mesh_buffers(map, bvh->verts, bvh->faces,
+				  node->prim_indices,
+				  node->totprim, node->vert_indices,
 				  node->uniq_verts,
 				  node->uniq_verts + node->face_verts);
 
 	BLI_ghash_free(map, NULL, NULL);
 }
 
+static void build_grids_leaf_node(PBVH *bvh, PBVHNode *node)
+{
+	node->draw_buffers =
+		GPU_build_grid_buffers(bvh->grids, node->prim_indices,
+				node->totprim, bvh->gridsize);
+}
+
 /* Recursively build a node in the tree
 
    vb is the voxel box around all of the primitives contained in
@@ -368,21 +384,25 @@ void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
 	BB cb_backing;
 
 	/* Decide whether this is a leaf or not */
-	if(count <= LEAF_LIMIT) {
+	// XXX adapt leaf limit for grids
+	if(count <= bvh->leaf_limit) {
 		bvh->nodes[node_index].flag |= PBVH_Leaf;
 
-		bvh->nodes[node_index].face_indices = bvh->face_indices + offset;
-		bvh->nodes[node_index].totface = count;
+		bvh->nodes[node_index].prim_indices = bvh->prim_indices + offset;
+		bvh->nodes[node_index].totprim = count;
 
 		/* Still need vb for searches */
 		BB_reset(&bvh->nodes[node_index].vb);
 		for(i = offset + count - 1; i >= offset; --i) {
 			BB_expand_with_bb(&bvh->nodes[node_index].vb,
 					  (BB*)(prim_bbc +
-						bvh->face_indices[i]));
+						bvh->prim_indices[i]));
 		}
 		
-		build_leaf_node(bvh, bvh->nodes + node_index);
+		if(bvh->faces)
+			build_mesh_leaf_node(bvh, bvh->nodes + node_index);
+		else
+			build_grids_leaf_node(bvh, bvh->nodes + node_index);
 		bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
 
 		/* Done with this subtree */
@@ -397,7 +417,7 @@ void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
 			cb = &cb_backing;
 			BB_reset(cb);
 			for(i = offset + count - 1; i >= offset; --i)
-				BB_expand(cb, prim_bbc[bvh->face_indices[i]].bcentroid);
+				BB_expand(cb, prim_bbc[bvh->prim_indices[i]].bcentroid);
 		}
 	}
 
@@ -405,16 +425,16 @@ void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
 
 	for(i = offset + count - 1; i >= offset; --i) {
 		BB_expand_with_bb(&bvh->nodes[node_index].vb,
-				  (BB*)(prim_bbc + bvh->face_indices[i]));
+				  (BB*)(prim_bbc + bvh->prim_indices[i]));
 	}
 
 	bvh->nodes[node_index].orig_vb= bvh->nodes[node_index].vb;
 
-	end = partition_indices(bvh->face_indices, offset, offset + count - 1,
+	end = partition_indices(bvh->prim_indices, offset, offset + count - 1,
 				axis,
 				(cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
 				prim_bbc);
-	check_partitioning(bvh->face_indices, offset, offset + count - 1,
+	check_partitioning(bvh->prim_indices, offset, offset + count - 1,
 			   axis,
 			   (cb->bmax[axis] + cb->bmin[axis]) * 0.5f,
 			   prim_bbc, end);
@@ -425,21 +445,18 @@ void build_sub(PBVH *bvh, int node_index, BB *cb, BBC *prim_bbc,
 		  prim_bbc, end, offset + count - end);
 }
 
-/* Do a full rebuild */
-void BLI_pbvh_build(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totvert)
+static void pbvh_build(PBVH *bvh, BB *cb, BBC *prim_bbc, int totprim)
 {
-	BBC *prim_bbc = NULL;
-	BB cb;
-	int i, j;
+	int i;
 
-	if(totface != bvh->totface) {
-		bvh->totface = totface;
+	if(totprim != bvh->totprim) {
+		bvh->totprim = totprim;
 		if(bvh->nodes) MEM_freeN(bvh->nodes);
-		if(bvh->face_indices) MEM_freeN(bvh->face_indices);
-		bvh->face_indices = MEM_callocN(sizeof(int) * totface,
-						"bvh face indices");
-		for(i = 0; i < totface; ++i)
-			bvh->face_indices[i] = i;
+		if(bvh->prim_indices) MEM_freeN(bvh->prim_indices);
+		bvh->prim_indices = MEM_callocN(sizeof(int) * totprim,
+						"bvh prim indices");
+		for(i = 0; i < totprim; ++i)
+			bvh->prim_indices[i] = i;
 		bvh->totnode = 0;
 		if(bvh->node_mem_count < 100) {
 			bvh->node_mem_count = 100;
@@ -449,10 +466,22 @@ void BLI_pbvh_build(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totv
 		}
 	}
 
+	bvh->totnode = 1;
+	build_sub(bvh, 0, cb, prim_bbc, 0, totprim);
+}
+
+/* Do a full rebuild with on Mesh data structure */
+void BLI_pbvh_build_mesh(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totvert)
+{
+	BBC *prim_bbc = NULL;
+	BB cb;
+	int i, j;
+
 	bvh->faces = faces;
 	bvh->verts = verts;
 	bvh->vert_bitmap = BLI_bitmap_new(totvert);
-	bvh->totvert= totvert;
+	bvh->totvert = totvert;
+	bvh->leaf_limit = LEAF_LIMIT;
 
 	BB_reset(&cb);
 
@@ -474,13 +503,50 @@ void BLI_pbvh_build(PBVH *bvh, MFace *faces, MVert *verts, int totface, int totv
 		BB_expand(&cb, bbc->bcentroid);
 	}
 
-	bvh->totnode = 1;
-	build_sub(bvh, 0, &cb, prim_bbc, 0, totface);
+	pbvh_build(bvh, &cb, prim_bbc, totface);
 
 	MEM_freeN(prim_bbc);
 	MEM_freeN(bvh->vert_bitmap);
 }
 
+/* Do a full rebuild with on Grids data structure */
+void BLI_pbvh_build_grids(PBVH *bvh, DMGridData **grids, int totgrid,
+	int gridsize, void **gridfaces)
+{
+	BBC *prim_bbc = NULL;
+	BB cb;
+	int i, j;
+
+	bvh->grids= grids;
+	bvh->gridfaces= gridfaces;
+	bvh->totgrid= totgrid;
+	bvh->gridsize= gridsize;
+	bvh->leaf_limit = MAX2(LEAF_LIMIT/((gridsize-1)*(gridsize-1)), 1);
+
+	BB_reset(&cb);
+
+	/* For each grid, store the AABB and the AABB centroid */
+	prim_bbc = MEM_mallocN(sizeof(BBC) * totgrid, "prim_bbc");
+
+	for(i = 0; i < totgrid; ++i) {
+		DMGridData *grid= grids[i];
+		BBC *bbc = prim_bbc + i;
+
+		BB_reset((BB*)bbc);
+
+		for(j = 0; j < gridsize*gridsize; ++j)
+			BB_expand((BB*)bbc, grid[j].co);
+
+		BBC_update_centroid(bbc);
+
+		BB_expand(&cb, bbc->bcentroid);
+	}
+
+	pbvh_build(bvh, &cb, prim_bbc, totgrid);
+
+	MEM_freeN(prim_bbc);
+}
+
 PBVH *BLI_pbvh_new(void)
 {
 	PBVH *bvh = MEM_callocN(sizeof(PBVH), "pbvh");
@@ -490,34 +556,27 @@ PBVH *BLI_pbvh_new(void)
 
 void BLI_pbvh_free(PBVH *bvh)
 {
+	PBVHNode *node;
 	int i;
 
 	for(i = 0; i < bvh->totnode; ++i) {
-		if(bvh->nodes[i].flag & PBVH_Leaf) {
-			GPU_free_buffers(bvh->nodes[i].draw_buffers);
-			MEM_freeN(bvh->nodes[i].vert_indices);
-			MEM_freeN(bvh->nodes[i].face_vert_indices);
+		node= &bvh->nodes[i];
+
+		if(node->flag & PBVH_Leaf) {
+			if(node->draw_buffers)
+				GPU_free_buffers(node->draw_buffers);
+			if(node->vert_indices)
+				MEM_freeN(node->vert_indices);
+			if(node->face_vert_indices)
+				MEM_freeN(node->face_vert_indices);
 		}
 	}
 
 	MEM_freeN(bvh->nodes);
-	MEM_freeN(bvh->face_indices);
+	MEM_freeN(bvh->prim_indices);
 	MEM_freeN(bvh);
 }
 
-void BLI_pbvh_set_source(PBVH *bvh, MVert *mvert, MFace *mface)
-{
-	bvh->verts = mvert;
-	bvh->faces = mface;
-}
-
-static void do_hit_callback(PBVH *bvh, PBVHNode *node,
-			    BLI_pbvh_HitCallback cb, void *data)
-{
-	if(cb)
-		cb(node, data);
-}
-
 static void pbvh_iter_begin(PBVHIter *iter, PBVH *bvh, BLI_pbvh_SearchCallback scb, void *search_data)
 {
 	iter->bvh= bvh;
@@ -646,7 +705,7 @@ void BLI_pbvh_search_callback(PBVH *bvh,
 
 	while((node=pbvh_iter_next(&iter)))
 		if(node->flag & PBVH_Leaf)
-			do_hit_callback(bvh, node, hcb, hit_data);
+			hcb(node, hit_data);
 
 	pbvh_iter_end(&iter);
 }
@@ -667,6 +726,9 @@ static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
 	float (*vnor)[3];
 	int n;
 
+	if(bvh->grids)
+		return;
+
 	/* could be per node to save some memory, but also means
 	   we have to store for each vertex which node it is in */
 	vnor= MEM_callocN(sizeof(float)*3*bvh->totvert, "bvh temp vnors");
@@ -688,8 +750,8 @@ static void pbvh_update_normals(PBVH *bvh, PBVHNode **nodes,
 		if((node->flag & PBVH_UpdateNormals)) {
 			int i, j, totface, *faces;
 
-			faces= node->face_indices;
-			totface= node->totface;
+			faces= node->prim_indices;
+			totface= node->totprim;
 
 			for(i = 0; i < totface; ++i) {
 				MFace *f= bvh->faces + faces[i];
@@ -792,11 +854,20 @@ static void pbvh_update_draw_buffers(PBVH *bvh, PBVHNode **nodes, int totnode)
 		node= nodes[n];
 
 		if(node->flag & PBVH_UpdateDrawBuffers) {
-			GPU_update_buffers(node->draw_buffers,
-					   bvh->verts,
-					   node->vert_indices,
-					   node->uniq_verts +
-					   node->face_verts);
+			if(bvh->grids) {
+				GPU_update_grid_buffers(node->draw_buffers,
+						   bvh->grids,
+						   node->prim_indices,
+						   node->totprim,
+						   bvh->gridsize);
+			}
+			else {
+				GPU_update_mesh_buffers(node->draw_buffers,
+						   bvh->verts,
+						   node->vert_indices,
+						   node->uniq_verts +
+						   node->face_verts);
+			}
 
 			node->flag &= ~PBVH_UpdateDrawBuffers;
 		}
@@ -877,6 +948,53 @@ void BLI_pbvh_redraw_BB(PBVH *bvh, float bb_min[3], float bb_max[3])
 	copy_v3_v3(bb_max, bb.bmax);
 }
 
+void BLI_pbvh_get_grid_updates(PBVH *bvh, void ***gridfaces, int *totface)
+{
+	PBVHIter iter;
+	PBVHNode *node;
+	GHashIterator *hiter;
+	GHash *map;
+	void *face, **faces;
+	int i, tot;
+
+	map = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp);
+
+	pbvh_iter_begin(&iter, bvh, NULL, NULL);
+
+	while((node=pbvh_iter_next(&iter))) {
+		if(node->flag & PBVH_UpdateNormals) {
+			for(i = 0; i < node->totprim; ++i) {
+				face= bvh->gridfaces[node->prim_indices[i]];
+				BLI_ghash_insert(map, face, face);
+			}
+
+			node->flag &= ~PBVH_UpdateNormals;
+		}
+	}
+
+	pbvh_iter_end(&iter);
+	
+	tot= BLI_ghash_size(map);
+	if(tot == 0) {
+		*totface= 0;
+		*gridfaces= NULL;
+		BLI_ghash_free(map, NULL, NULL);
+		return;
+	}
+
+	faces= MEM_callocN(sizeof(void*)*tot, "PBVH Grid Faces");
+
+	for(hiter = BLI_ghashIterator_new(map), i = 0;
+	    !BLI_ghashIterator_isDone(hiter);
+	    BLI_ghashIterator_step(hiter), ++i)
+		faces[i]= BLI_ghashIterator_getKey(hiter);
+
+	BLI_ghash_free(map, NULL, NULL);
+
+	*totface= tot;
+	*gridfaces= faces;
+}
+
 /***************************** Node Access ***********************************/
 
 void BLI_pbvh_node_mark_update(PBVHNode *node)
@@ -891,16 +1009,32 @@ void BLI_pbvh_node_get_verts(PBVHNode *node, int **vert_indices, int *totvert, i
 	if(allvert) *allvert= node->uniq_verts + node->face_verts;
 }
 
-void BLI_pbvh_node_get_faces(PBVHNode *node, int **face_indices, int **face_vert_indices, int *totface)
+void BLI_pbvh_node_num_verts(PBVH *bvh, PBVHNode *node, int *uniquevert, int *totvert)
 {
-	if(face_indices) *face_indices= node->face_indices;
-	if(face_vert_indices) *face_vert_indices= node->face_vert_indices;
-	if(totface) *totface= node->totface;
+	if(bvh->grids) {
+		*totvert= node->totprim*bvh->gridsize*bvh->gridsize;
+		*uniquevert= *totvert;
+	}
+	else {
+		*uniquevert= node->uniq_verts;
+		*totvert= node->uniq_verts + node->face_verts;
+	}
 }
 
-void *BLI_pbvh_node_get_draw_buffers(PBVHNode *node)
+void BLI_pbvh_node_get_grids(PBVH *bvh, PBVHNode *node, int **grid_indices, int *totgrid, int *maxgrid, int *gridsize)
 {
-	return node->draw_buffers;
+	if(bvh->grids) {
+		if(grid_indices) *grid_indices= node->prim_indices;
+		if(totgrid) *totgrid= node->totprim;
+		if(maxgrid) *maxgrid= bvh->totgrid;
+		if(gridsize) *gridsize= bvh->gridsize;
+	}
+	else {
+		*grid_indices= NULL;
+		*totgrid= 0;
+		*maxgrid= 0;
+		*gridsize= 0;
+	}
 }
 
 void BLI_pbvh_node_get_BB(PBVHNode *node, float bb_min[3], float bb_max[3])
@@ -936,7 +1070,7 @@ static int ray_aabb_intersect(PBVHNode *node, void *data_v)
 		BLI_pbvh_node_get_original_BB(node, bb_min, bb_max);
 	else
 		BLI_pbvh_node_get_BB(node, bb_min, bb_max);
-
+	
 	copy_v3_v3(bbox[0], bb_min);
 	copy_v3_v3(bbox[1], bb_max);
 
@@ -988,3 +1122,194 @@ void BLI_pbvh_raycast(PBVH *bvh, BLI_pbvh_HitCallback cb, void *data,
 	BLI_pbvh_search_callback(bvh, ray_aabb_intersect, &rcd, cb, data);
 }
 
+/* XXX: Code largely copied from bvhutils.c, could be unified */
+/* Returns 1 if a better intersection has been found */
+static int ray_face_intersection(float ray_start[3], float ray_normal[3],
+				 float *t0, float *t1, float *t2, float *t3,
+				 float *fdist)
+{
+	int hit = 0;
+
+	do
+	{	
+		float dist = FLT_MAX;
+			
+		if(!isect_ray_tri_v3(ray_start, ray_normal, t0, t1, t2,
+					 &dist, NULL))
+			dist = FLT_MAX;
+
+		if(dist >= 0 && dist < *fdist) {
+			hit = 1;
+			*fdist = dist;
+		}
+
+		t1 = t2;
+		t2 = t3;
+		t3 = NULL;
+
+	} while(t2);
+
+	return hit;
+}
+
+int BLI_pbvh_node_raycast(PBVH *bvh, PBVHNode *node, float (*origco)[3],
+	float ray_start[3], float ray_normal[3], float *dist)
+{
+	int hit= 0;
+
+	if(bvh->faces) {
+		MVert *vert = bvh->verts;
+		int *faces= node->prim_indices;
+		int *face_verts= node->face_vert_indices;
+		int totface= node->totprim;
+		int i;
+
+		for(i = 0; i < totface; ++i) {
+			MFace *f = bvh->faces + faces[i];
+
+			if(origco) {
+				/* intersect with backuped original coordinates */
+				hit |= ray_face_intersection(ray_start, ray_normal,
+							 origco[face_verts[i*4+0]],
+							 origco[face_verts[i*4+1]],
+							 origco[face_verts[i*4+2]],
+							 f->v4? origco[face_verts[i*4+3]]: NULL,
+							 dist);
+			}
+			else {
+				/* intersect with current coordinates */
+				hit |= ray_face_intersection(ray_start, ray_normal,
+							 vert[f->v1].co,
+							 vert[f->v2].co,
+							 vert[f->v3].co,
+							 f->v4 ? vert[f->v4].co : NULL,
+							 dist);
+			}
+		}
+	}
+	else {
+		int totgrid= node->totprim;
+		int gridsize= bvh->gridsize;
+		int i, x, y;
+
+		for(i = 0; i < totgrid; ++i) {
+			DMGridData *grid= bvh->grids[node->prim_indices[i]];
+
+			for(y = 0; y < gridsize-1; ++y) {
+				for(x = 0; x < gridsize-1; ++x) {
+					if(origco) {
+						hit |= ray_face_intersection(ray_start, ray_normal,
+									 origco[y*gridsize + x],
+									 origco[y*gridsize + x+1],
+									 origco[(y+1)*gridsize + x+1],
+									 origco[(y+1)*gridsize + x],
+									 dist);
+					}
+					else {
+						hit |= ray_face_intersection(ray_start, ray_normal,
+									 grid[y*gridsize + x].co,
+									 grid[y*gridsize + x+1].co,
+									 grid[(y+1)*gridsize + x+1].co,
+									 grid[(y+1)*gridsize + x].co,
+									 dist);
+					}
+				}
+			}
+
+			if(origco)
+				origco += gridsize*gridsize;
+		}
+	}
+
+	return hit;
+}
+
+#if 0
+static int nodes_drawn = 0;
+static int is_partial = 0;
+/* XXX: Just a temporary replacement for the real drawing code */
+static void draw_partial_cb(PBVHNode *node, void *data)
+
+	/* XXX: Just some quick code to show leaf nodes in different colors */
+	/*float col[3]; int i;
+	if(is_partial) {
+		col[0] = (rand() / (float)RAND_MAX); col[1] = col[2] = 0.6;
+	}
+	else {
+		srand((long long)data_v);
+		for(i = 0; i < 3; ++i)
+			col[i] = (rand() / (float)RAND_MAX) * 0.3 + 0.7;
+	}
+	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, col);
+
+	glColor3f(1, 0, 0);*/
+	GPU_draw_buffers(BLI_pbvh_node_get_draw_buffers(node));
+	++nodes_drawn;
+}
+#endif
+
+void BLI_pbvh_node_draw(PBVHNode *node, void *data)
+{
+	GPU_draw_buffers(node->draw_buffers);
+}
+
+/* Adapted from:
+   http://www.gamedev.net/community/forums/topic.asp?topic_id=512123
+   Returns true if the AABB is at least partially within the frustum
+   (ok, not a real frustum), false otherwise.
+*/
+int BLI_pbvh_node_planes_contain_AABB(PBVHNode *node, void *data)
+{
+	float (*planes)[4] = data;
+	int i, axis;
+	float vmin[3], vmax[3], bb_min[3], bb_max[3];
+
+	BLI_pbvh_node_get_BB(node, bb_min, bb_max);
+
+	for(i = 0; i < 4; ++i) { 
+		for(axis = 0; axis < 3; ++axis) {
+			if(planes[i][axis] > 0) { 
+				vmin[axis] = bb_min[axis];
+				vmax[axis] = bb_max[axis];
+			}
+			else {
+				vmin[axis] = bb_max[axis];
+				vmax[axis] = bb_min[axis];
+			}
+		}
+		
+		if(dot_v3v3(planes[i], vmin) + planes[i][3] > 0)
+			return 0;
+	} 
+
+	return 1;
+}
+
+void BLI_pbvh_draw(PBVH *bvh, float (*planes)[4], float (*face_nors)[3])
+{
+	BLI_pbvh_update(bvh, PBVH_UpdateNormals|PBVH_UpdateDrawBuffers, face_nors);
+
+	if(planes) {
+		BLI_pbvh_search_callback(bvh, BLI_pbvh_node_planes_contain_AABB,
+				planes, BLI_pbvh_node_draw, NULL);
+	}
+	else {
+		BLI_pbvh_search_callback(bvh, NULL, NULL, BLI_pbvh_node_draw, NULL);
+	}
+}
+
+void BLI_pbvh_node_verts_iter_init(PBVH *bvh, PBVHNode *node, PBVHVertexIter *vi, int mode)
+{
+	memset(vi, 0, sizeof(PBVHVertexIter));
+	vi->grids= bvh->grids;
+	vi->grid_indices= node->prim_indices;
+	vi->totgrid= (bvh->grids)? node->totprim: 1;
+	vi->gridsize= bvh->gridsize;
+
+	vi->totvert= node->uniq_verts;
+	if(mode == PBVH_ITER_ALL)
+		vi->totvert += node->face_verts;
+	vi->vert_indices= node->vert_indices;
+	vi->mverts= bvh->verts;
+}
+
diff --git a/source/blender/editors/sculpt_paint/sculpt.c b/source/blender/editors/sculpt_paint/sculpt.c
index 873bbc2baa6..c9e351458ad 100644
--- a/source/blender/editors/sculpt_paint/sculpt.c
+++ b/source/blender/editors/sculpt_paint/sculpt.c
@@ -267,13 +267,21 @@ typedef struct SculptUndoNode {
 	struct SculptUndoNode *next, *prev;
 
 	char idname[MAX_ID_NAME];	/* name instead of pointer*/
-	int maxvert;				/* to verify if totvert it still the same */
 	void *node;					/* only during push, not valid afterwards! */
 
 	float (*co)[3];
 	short (*no)[3];
-	int *index;
 	int totvert;
+
+	/* non-multires */
+	int maxvert;				/* to verify if totvert it still the same */
+	int *index;					/* to restore into right location */
+
+	/* multires */
+	int maxgrid;				/* same for grid */
+	int gridsize;				/* same for grid */
+	int totgrid;				/* to restore into right location */
+	int *grids;					/* to restore into right location */
 } SculptUndoNode;
 
 static void update_cb(PBVHNode *node, void *data)
@@ -284,33 +292,54 @@ static void update_cb(PBVHNode *node, void *data)
 static void sculpt_undo_restore(bContext *C, ListBase *lb)
 {
 	Object *ob = CTX_data_active_object(C);
+	DerivedMesh *dm = mesh_get_derived_final(CTX_data_scene(C), ob, 0);
 	SculptSession *ss = ob->sculpt;
 	SculptUndoNode *unode;
 	MVert *mvert;
 	MultiresModifierData *mmd;
 	int *index;
-	int i, totvert, update= 0;
+	int i, j, update= 0;
 
 	sculpt_update_mesh_elements(C, 0);
 
 	for(unode=lb->first; unode; unode=unode->next) {
 		if(!(strcmp(unode->idname, ob->id.name)==0))
 			continue;
-		if(ss->totvert != unode->maxvert)
-			continue;
 
-		index= unode->index;
-		totvert= unode->totvert;
-		mvert= ss->mvert;
+		if(unode->maxvert) {
+			/* regular mesh restore */
+			if(ss->totvert != unode->maxvert)
+				continue;
 
-		for(i=0; i<totvert; i++) {
-			float tmp[3];
+			index= unode->index;
+			mvert= ss->mvert;
 
-			copy_v3_v3(tmp, mvert[index[i]].co);
-			copy_v3_v3(mvert[index[i]].co, unode->co[i]);
-			copy_v3_v3(unode->co[i], tmp);
-
-			mvert[index[i]].flag |= ME_VERT_PBVH_UPDATE;
+			for(i=0; i<unode->totvert; i++) {
+				swap_v3_v3(mvert[index[i]].co, unode->co[i]);
+				mvert[index[i]].flag |= ME_VERT_PBVH_UPDATE;
+			}
+		}
+		else if(unode->maxgrid) {
+			/* multires restore */
+			DMGridData **grids, *grid;
+			float (*co)[3];
+			int gridsize;
+
+			if(dm->getNumGrids(dm) != unode->maxgrid)
+				continue;
+			if(dm->getGridSize(dm) != unode->gridsize)
+				continue;
+
+			grids= dm->getGridData(dm);
+			gridsize= dm->getGridSize(dm);
+
+			co = unode->co;
+			for(j=0; j<unode->totgrid; j++) {
+				grid= grids[unode->grids[j]];
+
+				for(i=0; i<gridsize*gridsize; i++, co++)
+					swap_v3_v3(grid[i].co, co[0]);
+			}
 		}
 
 		update= 1;
@@ -323,15 +352,8 @@ static void sculpt_undo_restore(bContext *C, ListBase *lb)
 		BLI_pbvh_search_callback(ss->tree, NULL, NULL, update_cb, NULL);
 		BLI_pbvh_update(ss->tree, PBVH_UpdateBB|PBVH_UpdateOriginalBB|PBVH_UpdateRedraw, NULL);
 
-		/* not really convinced this is correct .. */
-		if((mmd=sculpt_multires_active(ob))) {
-			mmd->undo_verts = ss->mvert;
-			mmd->undo_verts_tot = ss->totvert;
-			mmd->undo_signal = !!mmd->undo_verts;
-
-			multires_force_update(ob);
-			DAG_id_flush_update(&ob->id, OB_RECALC_DATA);
-		}
+		if((mmd=sculpt_multires_active(ob)))
+			multires_mark_as_modified(ob);
 	}
 }
 
@@ -346,6 +368,8 @@ static void sculpt_undo_free(ListBase *lb)
 			MEM_freeN(unode->no);
 		if(unode->index)
 			MEM_freeN(unode->index);
+		if(unode->grids)
+			MEM_freeN(unode->grids);
 	}
 }
 
@@ -369,9 +393,7 @@ static SculptUndoNode *sculpt_undo_push_node(SculptSession *ss, PBVHNode *node)
 	ListBase *lb= undo_paint_push_get_list(UNDO_PAINT_MESH);
 	Object *ob= ss->ob;
 	SculptUndoNode *unode;
-	int i, totvert, allvert, *verts;
-
-	BLI_pbvh_node_get_verts(node, &verts, &totvert, &allvert);
+	int totvert, allvert, totgrid, maxgrid, gridsize, *grids;
 
 	/* list is manipulated by multiple threads, so we lock */
 	BLI_lock_thread(LOCK_CUSTOM1);
@@ -385,23 +407,46 @@ static SculptUndoNode *sculpt_undo_push_node(SculptSession *ss, PBVHNode *node)
 	strcpy(unode->idname, ob->id.name);
 	unode->node= node;
 
+	BLI_pbvh_node_num_verts(ss->tree, node, &totvert, &allvert);
+	BLI_pbvh_node_get_grids(ss->tree, node, &grids, &totgrid, &maxgrid, &gridsize);
+
 	unode->totvert= totvert;
-	unode->maxvert= ss->totvert;
 	/* we will use this while sculpting, is mapalloc slow to access then? */
 	unode->co= MEM_mapallocN(sizeof(float)*3*allvert, "SculptUndoNode.co");
 	unode->no= MEM_mapallocN(sizeof(short)*3*allvert, "SculptUndoNode.no");
-	unode->index= MEM_mapallocN(sizeof(int)*allvert, "SculptUndoNode.index");
 	undo_paint_push_count_alloc(UNDO_PAINT_MESH, (sizeof(float)*3 + sizeof(short)*3 + sizeof(int))*allvert);
 	BLI_addtail(lb, unode);
 
+	if(maxgrid) {
+		/* multires */
+		unode->maxgrid= maxgrid;
+		unode->totgrid= totgrid;
+		unode->gridsize= gridsize;
+		unode->grids= MEM_mapallocN(sizeof(int)*totgrid, "SculptUndoNode.grids");
+	}
+	else {
+		/* regular mesh */
+		unode->maxvert= ss->totvert;
+		unode->index= MEM_mapallocN(sizeof(int)*allvert, "SculptUndoNode.index");
+	}
+
 	BLI_unlock_thread(LOCK_CUSTOM1);
 
 	/* copy threaded, hopefully this is the performance critical part */
-	memcpy(unode->index, verts, sizeof(int)*allvert);
-	for(i=0; i<allvert; i++) {
-		copy_v3_v3(unode->co[i], ss->mvert[verts[i]].co);
-		VECCOPY(unode->no[i], ss->mvert[verts[i]].no);
+	{
+		PBVHVertexIter vd;
+
+		BLI_pbvh_vertex_iter_begin(ss->tree, node, vd, PBVH_ITER_ALL) {
+			copy_v3_v3(unode->co[vd.i], vd.co);
+			if(vd.no) VECCOPY(unode->no[vd.i], vd.no)
+			else normal_float_to_short_v3(unode->no[vd.i], vd.fno);
+			if(vd.vert_indices) unode->index[vd.i]= vd.vert_indices[vd.i];
+		}
+		BLI_pbvh_vertex_iter_end;
 	}
+
+	if(unode->grids)
+		memcpy(unode->grids, grids, sizeof(int)*totgrid);
 	
 	return unode;
 }
@@ -428,57 +473,41 @@ static void sculpt_undo_push_end(SculptSession *ss)
 	undo_paint_push_end(UNDO_PAINT_MESH);
 }
 
-/************************ Looping Over Verts in a BVH Node *******************/
+void ED_sculpt_force_update(bContext *C)
+{
+	Object *ob= CTX_data_active_object(C);
 
-typedef struct SculptVertexData {
+	if(ob && (ob->mode & OB_MODE_SCULPT))
+		multires_force_update(ob);
+}
+
+/************************ Brush Testing *******************/
+
+typedef struct SculptBrushTest {
 	float radius_squared;
 	float location[3];
 
-	MVert *mvert;
-	int *verts;
-	float (*origvert)[3];
-	int i, index, totvert;
-
-	float *co;
-	float *origco;
-	short *no;
 	float dist;
-} SculptVertexData;
+} SculptBrushTest;
 
-static void sculpt_node_verts_init(Sculpt *sd, SculptSession *ss,
-	PBVHNode *node, float (*origvert)[3], SculptVertexData *vd)
+static void sculpt_brush_test_init(SculptSession *ss, SculptBrushTest *test)
 {
-	vd->radius_squared= ss->cache->radius*ss->cache->radius;
-	copy_v3_v3(vd->location, ss->cache->location);
-
-	vd->mvert= ss->mvert;
-	vd->origvert= origvert;
-	vd->i= -1;
-	BLI_pbvh_node_get_verts(node, &vd->verts, &vd->totvert, NULL);
+	test->radius_squared= ss->cache->radius*ss->cache->radius;
+	copy_v3_v3(test->location, ss->cache->location);
 }
 
-static int sculpt_node_verts_next(SculptVertexData *vd)
+static int sculpt_brush_test(SculptBrushTest *test, float co[3])
 {
-	vd->i++;
-
-	while(vd->i < vd->totvert) {
-		float delta[3], dsq;
+	float distsq, delta[3];
 
-		vd->index= vd->verts[vd->i];
-		vd->co= vd->mvert[vd->index].co;
-		vd->origco= (vd->origvert)? vd->origvert[vd->i]: vd->co;
-		vd->no= vd->mvert[vd->index].no;
-		sub_v3_v3v3(delta, vd->origco, vd->location);
-		dsq = INPR(delta, delta);
+	sub_v3_v3v3(delta, co, test->location);
+	distsq = INPR(delta, delta);
 
-		if(dsq < vd->radius_squared) {
-			vd->dist = sqrt(dsq);
-			return 1;
-		}
-
-		vd->i++;
+	if(distsq < test->radius_squared) {
+		test->dist = sqrt(distsq);
+		return 1;
 	}
-	
+
 	return 0;
 }
 
@@ -667,7 +696,6 @@ static int sculpt_search_sphere_cb(PBVHNode *node, void *data_v)
 	float t[3], bb_min[3], bb_max[3];
 	int i;
 
-	//BLI_pbvh_node_get_original_BB(node, bb_min, bb_max);
 	BLI_pbvh_node_get_BB(node, bb_min, bb_max);
 
 	for(i = 0; i < 3; ++i) {
@@ -700,12 +728,8 @@ static void sculpt_clip(Sculpt *sd, SculptSession *ss, float *co, const float va
 	}		
 }
 
-static void add_norm_if(float view_vec[3], float out[3], float out_flip[3], const short no[3])
+static void add_norm_if(float view_vec[3], float out[3], float out_flip[3], float fno[3])
 {
-	float fno[3] = {no[0], no[1], no[2]};
-
-	normalize_v3(fno);
-
 	if((dot_v3v3(view_vec, fno)) > 0) {
 		add_v3_v3v3(out, out, fno);
 	} else {
@@ -716,6 +740,7 @@ static void add_norm_if(float view_vec[3], float out[3], float out_flip[3], cons
 /* For draw/layer/flatten; finds average normal for all active vertices */
 static void calc_area_normal(Sculpt *sd, SculptSession *ss, float area_normal[3], PBVHNode **nodes, int totnode)
 {
+	PBVH *bvh= ss->tree;
 	StrokeCache *cache = ss->cache;
 	const int view = 0; /* XXX: should probably be a flag, not number: brush_type==SCULPT_TOOL_DRAW ? sculptmode_brush()->view : 0; */
 	float out[3] = {0.0f, 0.0f, 0.0f};
@@ -728,23 +753,39 @@ static void calc_area_normal(Sculpt *sd, SculptSession *ss, float area_normal[3]
 	/* threaded loop over nodes */
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		SculptUndoNode *unode;
+		float fno[3];
 		float nout[3] = {0.0f, 0.0f, 0.0f};
 		float nout_flip[3] = {0.0f, 0.0f, 0.0f};
 		
 		// XXX push instead of get for thread safety in draw
 		// brush .. lame, but also not harmful really
 		unode= sculpt_undo_push_node(ss, nodes[n]);
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
+		sculpt_brush_test_init(ss, &test);
 
-		if(unode && ss->cache->original) {
-			while(sculpt_node_verts_next(&vd))
-				add_norm_if(out_dir, nout, nout_flip, unode->no[vd.i]);
+		if(ss->cache->original) {
+			BLI_pbvh_vertex_iter_begin(bvh, nodes[n], vd, PBVH_ITER_UNIQUE) {
+				if(sculpt_brush_test(&test, unode->co[vd.i])) {
+					normal_short_to_float_v3(fno, unode->no[vd.i]);
+					add_norm_if(out_dir, nout, nout_flip, fno);
+				}
+			}
+			BLI_pbvh_vertex_iter_end;
 		}
 		else {
-			while(sculpt_node_verts_next(&vd))
-				add_norm_if(out_dir, nout, nout_flip, ss->mvert[vd.index].no);
+			BLI_pbvh_vertex_iter_begin(bvh, nodes[n], vd, PBVH_ITER_UNIQUE) {
+				if(sculpt_brush_test(&test, vd.co)) {
+					if(vd.no) {
+						normal_short_to_float_v3(fno, vd.no);
+						add_norm_if(out_dir, nout, nout_flip, fno);
+					}
+					else
+						add_norm_if(out_dir, nout, nout_flip, vd.fno);
+				}
+			}
+			BLI_pbvh_vertex_iter_end;
 		}
 
 		{
@@ -787,21 +828,25 @@ static void do_draw_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int t
 	/* threaded loop over nodes */
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		
 		sculpt_undo_push_node(ss, nodes[n]);
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
+		sculpt_brush_test_init(ss, &test);
 
-		while(sculpt_node_verts_next(&vd)) {
-			/* offset vertex */
-			const float fade = tex_strength(ss, brush, vd.co, vd.dist);
-			const float val[3]= {vd.co[0] + offset[0]*fade,
-								 vd.co[1] + offset[1]*fade,
-								 vd.co[2] + offset[2]*fade};
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				/* offset vertex */
+				float fade = tex_strength(ss, brush, vd.co, test.dist);
+				float val[3]= {vd.co[0] + offset[0]*fade,
+							   vd.co[1] + offset[1]*fade,
+							   vd.co[2] + offset[2]*fade};
 
-			sculpt_clip(sd, ss, vd.co, val);
-			ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+				sculpt_clip(sd, ss, vd.co, val);
+				if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
@@ -860,23 +905,27 @@ static void do_smooth_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int
 	for(iteration = 0; iteration < 2; ++iteration) {
 		#pragma omp parallel for private(n) schedule(static)
 		for(n=0; n<totnode; n++) {
-			SculptVertexData vd;
+			PBVHVertexIter vd;
+			SculptBrushTest test;
 			
 			sculpt_undo_push_node(ss, nodes[n]);
-			sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
-
-			while(sculpt_node_verts_next(&vd)) {
-				const float fade = tex_strength(ss, brush, vd.co, vd.dist)*bstrength;
-				float avg[3], val[3];
-				
-				neighbor_average(ss, avg, vd.index);
-				val[0] = vd.co[0]+(avg[0]-vd.co[0])*fade;
-				val[1] = vd.co[1]+(avg[1]-vd.co[1])*fade;
-				val[2] = vd.co[2]+(avg[2]-vd.co[2])*fade;
-				
-				sculpt_clip(sd, ss, vd.co, val);			
-				ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+			sculpt_brush_test_init(ss, &test);
+
+			BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+				if(sculpt_brush_test(&test, vd.co)) {
+					float fade = tex_strength(ss, brush, vd.co, test.dist)*bstrength;
+					float avg[3], val[3];
+					
+					neighbor_average(ss, avg, vd.vert_indices[vd.i]);
+					val[0] = vd.co[0]+(avg[0]-vd.co[0])*fade;
+					val[1] = vd.co[1]+(avg[1]-vd.co[1])*fade;
+					val[2] = vd.co[2]+(avg[2]-vd.co[2])*fade;
+					
+					sculpt_clip(sd, ss, vd.co, val);			
+					if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+				}
 			}
+			BLI_pbvh_vertex_iter_end;
 
 			BLI_pbvh_node_mark_update(nodes[n]);
 		}
@@ -891,20 +940,24 @@ static void do_pinch_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int
 
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		
 		sculpt_undo_push_node(ss, nodes[n]);
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
-
-		while(sculpt_node_verts_next(&vd)) {
-			const float fade = tex_strength(ss, brush, vd.co, vd.dist)*bstrength;
-			const float val[3]= {vd.co[0]+(vd.location[0]-vd.co[0])*fade,
-								 vd.co[1]+(vd.location[1]-vd.co[1])*fade,
-								 vd.co[2]+(vd.location[2]-vd.co[2])*fade};
-			
-			sculpt_clip(sd, ss, vd.co, val);			
-			ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+		sculpt_brush_test_init(ss, &test);
+
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				float fade = tex_strength(ss, brush, vd.co, test.dist)*bstrength;
+				float val[3]= {vd.co[0]+(test.location[0]-vd.co[0])*fade,
+							   vd.co[1]+(test.location[1]-vd.co[1])*fade,
+							   vd.co[2]+(test.location[2]-vd.co[2])*fade};
+				
+				sculpt_clip(sd, ss, vd.co, val);			
+				if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
@@ -921,26 +974,29 @@ static void do_grab_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int t
 
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
-		SculptUndoNode *unode;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		float (*origco)[3];
 		
-		unode= sculpt_undo_push_node(ss, nodes[n]);
-		origco= unode->co;
-		sculpt_node_verts_init(sd, ss, nodes[n], origco, &vd);
-
-		while(sculpt_node_verts_next(&vd)) {
-			const float fade = tex_strength(ss, brush, origco[vd.i], vd.dist)*bstrength;
-			const float add[3]= {vd.co[0]+fade*grab_delta[0],
-					     vd.co[1]+fade*grab_delta[1],
-					     vd.co[2]+fade*grab_delta[2]};
-
-			sculpt_clip(sd, ss, vd.co, add);			
-			ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+		origco= sculpt_undo_push_node(ss, nodes[n])->co;
+		sculpt_brush_test_init(ss, &test);
+
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, origco[vd.i])) {
+				float fade = tex_strength(ss, brush, origco[vd.i], test.dist)*bstrength;
+				float add[3]= {vd.co[0]+fade*grab_delta[0],
+							   vd.co[1]+fade*grab_delta[1],
+							   vd.co[2]+fade*grab_delta[2]};
+
+				sculpt_clip(sd, ss, vd.co, add);			
+				if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
+
 }
 
 static void do_layer_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int totnode)
@@ -962,40 +1018,43 @@ static void do_layer_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int
 
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
-		SculptUndoNode *unode;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		float (*origco)[3];
 		
-		unode= sculpt_undo_push_node(ss, nodes[n]);
-		origco= unode->co;
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
+		origco= sculpt_undo_push_node(ss, nodes[n])->co;
+		sculpt_brush_test_init(ss, &test);
+
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				float fade = tex_strength(ss, brush, vd.co, test.dist)*bstrength;
+				int index= vd.vert_indices[vd.i];
+				float *disp= &ss->layer_disps[index];
+				float val[3];
+				
+				*disp+= fade;
+				
+				/* Don't let the displacement go past the limit */
+				if((lim < 0 && *disp < lim) || (lim > 0 && *disp > lim))
+					*disp = lim;
+				
+				if(ss->layer_co && (brush->flag & BRUSH_PERSISTENT)) {
+					/* persistent base */
+					val[0] = ss->layer_co[index][0] + (*disp)*offset[0];
+					val[1] = ss->layer_co[index][1] + (*disp)*offset[1];
+					val[2] = ss->layer_co[index][2] + (*disp)*offset[2];
+				}
+				else {
+					val[0] = origco[vd.i][0] + (*disp)*offset[0];
+					val[1] = origco[vd.i][1] + (*disp)*offset[1];
+					val[2] = origco[vd.i][2] + (*disp)*offset[2];
+				}
 
-		while(sculpt_node_verts_next(&vd)) {
-			const float fade = tex_strength(ss, brush, vd.co, vd.dist)*bstrength;
-			float *disp= &ss->layer_disps[vd.index];
-			float val[3];
-			
-			*disp+= fade;
-			
-			/* Don't let the displacement go past the limit */
-			if((lim < 0 && *disp < lim) || (lim > 0 && *disp > lim))
-				*disp = lim;
-			
-			if(ss->layer_co && (brush->flag & BRUSH_PERSISTENT)) {
-				/* persistent base */
-				val[0] = ss->layer_co[vd.index][0] + (*disp)*offset[0];
-				val[1] = ss->layer_co[vd.index][1] + (*disp)*offset[1];
-				val[2] = ss->layer_co[vd.index][2] + (*disp)*offset[2];
-			}
-			else {
-				val[0] = origco[vd.i][0] + (*disp)*offset[0];
-				val[1] = origco[vd.i][1] + (*disp)*offset[1];
-				val[2] = origco[vd.i][2] + (*disp)*offset[2];
+				sculpt_clip(sd, ss, vd.co, val);
+				if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
 			}
-
-			sculpt_clip(sd, ss, vd.co, val);
-			ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
@@ -1009,27 +1068,31 @@ static void do_inflate_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, in
 
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		
 		sculpt_undo_push_node(ss, nodes[n]);
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
+		sculpt_brush_test_init(ss, &test);
 
-		while(sculpt_node_verts_next(&vd)) {
-			const float fade = tex_strength(ss, brush, vd.co, vd.dist)*bstrength;
-			float add[3];
-			
-			add[0]= vd.no[0]/32767.0f;
-			add[1]= vd.no[1]/32767.0f;
-			add[2]= vd.no[2]/32767.0f;
-			mul_v3_fl(add, fade * ss->cache->radius);
-			add[0]*= ss->cache->scale[0];
-			add[1]*= ss->cache->scale[1];
-			add[2]*= ss->cache->scale[2];
-			add_v3_v3v3(add, add, vd.co);
-			
-			sculpt_clip(sd, ss, vd.co, add);
-			ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				float fade = tex_strength(ss, brush, vd.co, test.dist)*bstrength;
+				float add[3];
+				
+				add[0]= vd.no[0]/32767.0f;
+				add[1]= vd.no[1]/32767.0f;
+				add[2]= vd.no[2]/32767.0f;
+				mul_v3_fl(add, fade * ss->cache->radius);
+				add[0]*= ss->cache->scale[0];
+				add[1]*= ss->cache->scale[1];
+				add[2]*= ss->cache->scale[2];
+				add_v3_v3v3(add, add, vd.co);
+				
+				sculpt_clip(sd, ss, vd.co, add);
+				if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
@@ -1038,36 +1101,42 @@ static void do_inflate_brush(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, in
 static void calc_flatten_center(Sculpt *sd, SculptSession *ss, PBVHNode **nodes, int totnode, float co[3])
 {
 	float outer_dist[FLATTEN_SAMPLE_SIZE];
-	int outer_index[FLATTEN_SAMPLE_SIZE];
+	float outer_co[FLATTEN_SAMPLE_SIZE][3];
 	int i, n;
 	
 	for(i = 0; i < FLATTEN_SAMPLE_SIZE; ++i) {
-		outer_index[i] = 0;
+		zero_v3(outer_co[i]);
 		outer_dist[i]= -1.0f;
 	}
 		
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
-
-		while(sculpt_node_verts_next(&vd)) {
-			for(i = 0; i < FLATTEN_SAMPLE_SIZE; ++i) {
-				if(vd.dist > outer_dist[i]) {
-					outer_index[i] = vd.index;
-					outer_dist[i] = vd.dist;
-					break;
+		sculpt_undo_push_node(ss, nodes[n]);
+		sculpt_brush_test_init(ss, &test);
+
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				for(i = 0; i < FLATTEN_SAMPLE_SIZE; ++i) {
+					if(test.dist > outer_dist[i]) {
+						copy_v3_v3(outer_co[i], vd.co);
+						outer_dist[i] = test.dist;
+						break;
+					}
 				}
 			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
 	
 	co[0] = co[1] = co[2] = 0.0f;
 	for(i = 0; i < FLATTEN_SAMPLE_SIZE; ++i)
-		add_v3_v3v3(co, co, ss->mvert[outer_index[i]].co);
+		if(outer_dist[i] >= 0.0f)
+			add_v3_v3v3(co, co, outer_co[i]);
 	mul_v3_fl(co, 1.0f / FLATTEN_SAMPLE_SIZE);
 }
 
@@ -1122,41 +1191,45 @@ static void do_flatten_clay_brush(Sculpt *sd, SculptSession *ss, PBVHNode **node
 
 	#pragma omp parallel for private(n) schedule(static)
 	for(n=0; n<totnode; n++) {
-		SculptVertexData vd;
+		PBVHVertexIter vd;
+		SculptBrushTest test;
 		
 		sculpt_undo_push_node(ss, nodes[n]);
-		sculpt_node_verts_init(sd, ss, nodes[n], NULL, &vd);
+		sculpt_brush_test_init(ss, &test);
 
-		while(sculpt_node_verts_next(&vd)) {
-			float intr[3], val[3];
-			
-			if(!clay || plane_point_side(vd.co, area_normal, cntr2, flip)) {
-				const float fade = tex_strength(ss, brush, vd.co, vd.dist)*bstrength;
-
-				/* Find the intersection between squash-plane and vertex (along the area normal) */		
-				point_plane_project(intr, vd.co, area_normal, cntr);
-
-				sub_v3_v3v3(val, intr, vd.co);
-
-				if(clay) {
-					if(bstr > FLT_EPSILON)
-						mul_v3_fl(val, fade / bstr);
+		BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+			if(sculpt_brush_test(&test, vd.co)) {
+				float intr[3], val[3];
+				
+				if(!clay || plane_point_side(vd.co, area_normal, cntr2, flip)) {
+					const float fade = tex_strength(ss, brush, vd.co, test.dist)*bstrength;
+
+					/* Find the intersection between squash-plane and vertex (along the area normal) */		
+					point_plane_project(intr, vd.co, area_normal, cntr);
+
+					sub_v3_v3v3(val, intr, vd.co);
+
+					if(clay) {
+						if(bstr > FLT_EPSILON)
+							mul_v3_fl(val, fade / bstr);
+						else
+							mul_v3_fl(val, fade);
+						/* Clay displacement */
+						val[0]+=area_normal[0] * ss->cache->scale[0]*fade;
+						val[1]+=area_normal[1] * ss->cache->scale[1]*fade;
+						val[2]+=area_normal[2] * ss->cache->scale[2]*fade;
+					}
 					else
-						mul_v3_fl(val, fade);
-					/* Clay displacement */
-					val[0]+=area_normal[0] * ss->cache->scale[0]*fade;
-					val[1]+=area_normal[1] * ss->cache->scale[1]*fade;
-					val[2]+=area_normal[2] * ss->cache->scale[2]*fade;
-				}
-				else
-					mul_v3_fl(val, fabs(fade));
+						mul_v3_fl(val, fabs(fade));
 
-				add_v3_v3v3(val, val, vd.co);
+					add_v3_v3v3(val, val, vd.co);
 
-				sculpt_clip(sd, ss, vd.co, val);
-				ss->mvert[vd.index].flag |= ME_VERT_PBVH_UPDATE;
+					sculpt_clip(sd, ss, vd.co, val);
+					if(vd.mvert) vd.mvert->flag |= ME_VERT_PBVH_UPDATE;
+				}
 			}
 		}
+		BLI_pbvh_vertex_iter_end;
 
 		BLI_pbvh_node_mark_update(nodes[n]);
 	}
@@ -1312,7 +1385,7 @@ char sculpt_modifiers_active(Object *ob)
 	ModifierData *md;
 	
 	for(md= modifiers_getVirtualModifierList(ob); md; md= md->next) {
-		if(md->mode & eModifierMode_Realtime && md->type != eModifierType_Multires)
+		if(modifier_isEnabled(md, eModifierMode_Realtime) && md->type != eModifierType_Multires)
 			return 1;
 	}
 	
@@ -1346,9 +1419,7 @@ struct MultiresModifierData *sculpt_multires_active(Object *ob)
 void sculpt_update_mesh_elements(bContext *C, int need_fmap)
 {
 	Object *ob = CTX_data_active_object(C);
-	DerivedMesh *dm =
-		mesh_get_derived_final(CTX_data_scene(C), ob,
-				       CTX_wm_view3d(C)->customdata_mask);
+	DerivedMesh *dm = mesh_get_derived_final(CTX_data_scene(C), ob, 0);
 	SculptSession *ss = ob->sculpt;
 	
 	if((ss->multires = sculpt_multires_active(ob))) {
@@ -1611,36 +1682,6 @@ static void sculpt_update_cache_variants(Sculpt *sd, SculptSession *ss, struct P
 	}
 }
 
-/* XXX: Code largely copied from bvhutils.c, should be unified */
-/* Returns 1 if a better intersection has been found */
-static int ray_face_intersection(float ray_start[3], float ray_normal[3],
-				 float *t0, float *t1, float *t2, float *t3,
-				 float *fdist)
-{
-	int hit = 0;
-
-	do
-	{	
-		float dist = FLT_MAX;
-			
-		if(!isect_ray_tri_v3(ray_start, ray_normal, t0, t1, t2,
-					 &dist, NULL))
-			dist = FLT_MAX;
-
-		if(dist >= 0 && dist < *fdist) {
-			hit = 1;
-			*fdist = dist;
-		}
-
-		t1 = t2;
-		t2 = t3;
-		t3 = NULL;
-
-	} while(t2);
-
-	return hit;
-}
-
 typedef struct {
 	SculptSession *ss;
 	float *ray_start, *ray_normal;
@@ -1652,53 +1693,16 @@ typedef struct {
 void sculpt_raycast_cb(PBVHNode *node, void *data_v)
 {
 	SculptRaycastData *srd = data_v;
-	MVert *vert = srd->ss->mvert;
-	int i, totface, *faces, *face_verts;
+	float (*origco)[3]= NULL;
 
 	if(srd->original && srd->ss->cache) {
-		SculptUndoNode *unode;
-
-		unode= sculpt_undo_get_node(srd->ss, node);
-
-		if(unode) {
-			/* intersect with coordinates from before we started stroke */
-			BLI_pbvh_node_get_faces(node, &faces, &face_verts, &totface);
-
-			for(i = 0; i < totface; ++i) {
-				MFace *f = srd->ss->mface + faces[i];
-				/*if(face_verts[i*4 + 0] >= unode->totvert) abort();
-				if(face_verts[i*4 + 1] >= unode->totvert) abort();
-				if(face_verts[i*4 + 2] >= unode->totvert) abort();
-				if(f->v4 && face_verts[i*4 + 3] >= unode->totvert) abort();*/
-
-				if(ray_face_intersection(srd->ray_start, srd->ray_normal,
-							 unode->co[face_verts[i*4+0]],
-							 unode->co[face_verts[i*4+1]],
-							 unode->co[face_verts[i*4+2]],
-							 f->v4? unode->co[face_verts[i*4+3]]: NULL,
-							 &srd->dist)) {
-					srd->hit = faces[i];
-				}
-			}
-
-			return;
-		}
+		/* intersect with coordinates from before we started stroke */
+		SculptUndoNode *unode= sculpt_undo_get_node(srd->ss, node);
+		origco= (unode)? unode->co: NULL;
 	}
 
-	/* intersect with current coordinates */
-	BLI_pbvh_node_get_faces(node, &faces, NULL, &totface);
-
-	for(i = 0; i < totface; ++i) {
-		MFace *f = srd->ss->mface + faces[i];
-		if(ray_face_intersection(srd->ray_start, srd->ray_normal,
-					 vert[f->v1].co,
-					 vert[f->v2].co,
-					 vert[f->v3].co,
-					 f->v4 ? vert[f->v4].co : NULL,
-					 &srd->dist)) {
-			srd->hit = faces[i];
-		}
-	}
+	srd->hit |= BLI_pbvh_node_raycast(srd->ss->tree, node, origco,
+		srd->ray_start, srd->ray_normal, &srd->dist);
 }
 
 /* Do a raycast in the tree to find the 3d brush location
@@ -1721,7 +1725,7 @@ int sculpt_stroke_get_location(bContext *C, struct PaintStroke *stroke, float ou
 	srd.ray_start = ray_start;
 	srd.ray_normal = ray_normal;
 	srd.dist = FLT_MAX;
-	srd.hit = -1;
+	srd.hit = 0;
 	srd.original = (cache)? cache->original: 0;
 	BLI_pbvh_raycast(ss->tree, sculpt_raycast_cb, &srd,
 		     ray_start, ray_normal, srd.original);
@@ -1730,7 +1734,7 @@ int sculpt_stroke_get_location(bContext *C, struct PaintStroke *stroke, float ou
 	mul_v3_fl(out, srd.dist);
 	add_v3_v3v3(out, out, ray_start);
 
-	return srd.hit != -1;
+	return srd.hit;
 }
 
 /* Initialize stroke operator properties */
@@ -1794,20 +1798,25 @@ static void sculpt_restore_mesh(Sculpt *sd, SculptSession *ss)
 
 	/* Restore the mesh before continuing with anchored stroke */
 	if(brush->flag & BRUSH_ANCHORED) {
-		ListBase *lb= undo_paint_push_get_list(UNDO_PAINT_MESH);
-		SculptUndoNode *unode;
-
-		/* this could benefit from multithreading... */
+		PBVHNode **nodes;
+		int n, totnode;
 
-		for(unode = lb->first; unode; unode = unode->next) {
-			float (*co)[3]= unode->co;
-			short (*no)[3]= unode->no;
-			int *index= unode->index;
-			int totvert= unode->totvert;
+		BLI_pbvh_search_gather(ss->tree, NULL, NULL, &nodes, &totnode);
 
-			for(i = 0; i < totvert; ++i) {
-				copy_v3_v3(ss->mvert[index[i]].co, co[i]);
-				VECCOPY(ss->mvert[index[i]].no, no[i]);
+		#pragma omp parallel for private(n) schedule(static)
+		for(n=0; n<totnode; n++) {
+			SculptUndoNode *unode;
+			
+			unode= sculpt_undo_get_node(ss, nodes[n]);
+			if(unode) {
+				PBVHVertexIter vd;
+
+				BLI_pbvh_vertex_iter_begin(ss->tree, nodes[n], vd, PBVH_ITER_UNIQUE) {
+					copy_v3_v3(vd.co, unode->co[vd.i]);
+					if(vd.no) VECCOPY(vd.no, unode->no[vd.i])
+					else normal_short_to_float_v3(vd.fno, unode->no[vd.i]);
+				}
+				BLI_pbvh_vertex_iter_end;
 			}
 		}
 
@@ -1831,14 +1840,8 @@ static void sculpt_flush_update(bContext *C)
 	rcti r;
 	int redraw = 0;
 
-	if(mmd) {
-		if(mmd->undo_verts && mmd->undo_verts != ss->mvert)
-			MEM_freeN(mmd->undo_verts);
-		
-		mmd->undo_verts = ss->mvert;
-		mmd->undo_verts_tot = ss->totvert;
+	if(mmd)
 		multires_mark_as_modified(ob);
-	}
 
 	BLI_pbvh_update(ss->tree, PBVH_UpdateBB, NULL);
 	redraw = sculpt_get_redraw_rect(ar, CTX_wm_region_view3d(C), ob, &r);
@@ -2024,10 +2027,14 @@ static int sculpt_toggle_mode(bContext *C, wmOperator *op)
 {
 	ToolSettings *ts = CTX_data_tool_settings(C);
 	Object *ob = CTX_data_active_object(C);
+	//MultiresModifierData *mmd = sculpt_multires_active(ob);
 
 	if(ob->mode & OB_MODE_SCULPT) {
 		multires_force_update(ob);
 
+		/*if(mmd && mmd->sculptlvl != mmd->lvl)
+			DAG_id_flush_update(&ob->id, OB_RECALC_DATA);*/
+
 		/* Leave sculptmode */
 		ob->mode &= ~OB_MODE_SCULPT;
 
@@ -2035,8 +2042,10 @@ static int sculpt_toggle_mode(bContext *C, wmOperator *op)
 	}
 	else {
 		/* Enter sculptmode */
-
 		ob->mode |= OB_MODE_SCULPT;
+
+		/*if(mmd && mmd->sculptlvl != mmd->lvl)
+			DAG_id_flush_update(&ob->id, OB_RECALC_DATA);*/
 		
 		/* Create persistent sculpt mode data */
 		if(!ts->sculpt)
diff --git a/source/blender/gpu/gpu_buffers.h b/source/blender/gpu/gpu_buffers.h
index eceb0cdeca4..5531ccc813b 100644
--- a/source/blender/gpu/gpu_buffers.h
+++ b/source/blender/gpu/gpu_buffers.h
@@ -126,13 +126,17 @@ GPUDrawObject *GPU_drawobject_new( struct DerivedMesh *dm );
 void GPU_drawobject_free( struct DerivedMesh *dm );
 
 /* Buffers for non-DerivedMesh drawing */
-void *GPU_build_buffers(struct GHash *map, struct MVert *mvert,
+void *GPU_build_mesh_buffers(struct GHash *map, struct MVert *mvert,
 			struct MFace *mface, int *face_indices,
 			int totface, int *vert_indices, int uniq_verts,
 			int totvert);
-void GPU_draw_buffers(void *buffers);
-void GPU_update_buffers(void *buffers, struct MVert *mvert,
+void GPU_update_mesh_buffers(void *buffers, struct MVert *mvert,
 			int *vert_indices, int totvert);
+void *GPU_build_grid_buffers(struct DMGridData **grids,
+	int *grid_indices, int totgrid, int gridsize);
+void GPU_update_grid_buffers(void *buffers_v, struct DMGridData **grids,
+	int *grid_indices, int totgrid, int gridsize);
+void GPU_draw_buffers(void *buffers);
 void GPU_free_buffers(void *buffers);
 
 /* called before drawing */
diff --git a/source/blender/gpu/intern/gpu_buffers.c b/source/blender/gpu/intern/gpu_buffers.c
index a6adf8a16a1..65b81835cf9 100644
--- a/source/blender/gpu/intern/gpu_buffers.c
+++ b/source/blender/gpu/intern/gpu_buffers.c
@@ -30,6 +30,7 @@
  * ***** END GPL LICENSE BLOCK *****
  */
 
+#include <limits.h>
 #include <string.h>
 
 #include "GL/glew.h"
@@ -388,11 +389,12 @@ typedef struct {
 } VertexBufferFormat;
 
 typedef struct {
-	unsigned int vert_buf, tri_buf;
-	unsigned short tot_tri;
+	GLuint vert_buf, index_buf;
+	GLenum index_type;
+	unsigned int tot_tri, tot_quad;
 } GPU_Buffers;
 
-void GPU_update_buffers(void *buffers_v, MVert *mvert,
+void GPU_update_mesh_buffers(void *buffers_v, MVert *mvert,
 			int *vert_indices, int totvert)
 {
 	GPU_Buffers *buffers = buffers_v;
@@ -414,11 +416,9 @@ void GPU_update_buffers(void *buffers_v, MVert *mvert,
 		memcpy(out->no, v->no, sizeof(short) * 3);
 	}
 	glUnmapBufferARB(GL_ARRAY_BUFFER_ARB);
-
-	//printf("node updated %p\n", buffers_v);
 }
 
-void *GPU_build_buffers(GHash *map, MVert *mvert, MFace *mface,
+void *GPU_build_mesh_buffers(GHash *map, MVert *mvert, MFace *mface,
 			int *face_indices, int totface,
 			int *vert_indices, int tot_uniq_verts,
 			int totvert)
@@ -428,14 +428,15 @@ void *GPU_build_buffers(GHash *map, MVert *mvert, MFace *mface,
 	int i, j, k, tottri;
 
 	buffers = MEM_callocN(sizeof(GPU_Buffers), "GPU_Buffers");
+	buffers->index_type = GL_UNSIGNED_SHORT;
 
 	/* Count the number of triangles */
 	for(i = 0, tottri = 0; i < totface; ++i)
 		tottri += mface[face_indices[i]].v4 ? 2 : 1;
 
 	/* Generate index buffer object */
-	glGenBuffersARB(1, &buffers->tri_buf);
-	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffers->tri_buf);
+	glGenBuffersARB(1, &buffers->index_buf);
+	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffers->index_buf);
 	glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB,
 		     sizeof(unsigned short) * tottri * 3, NULL, GL_STATIC_DRAW_ARB);
 
@@ -470,24 +471,129 @@ void *GPU_build_buffers(GHash *map, MVert *mvert, MFace *mface,
 
 	/* Build VBO */
 	glGenBuffersARB(1, &buffers->vert_buf);
-	GPU_update_buffers(buffers, mvert, vert_indices, totvert);
+	GPU_update_mesh_buffers(buffers, mvert, vert_indices, totvert);
 
 	buffers->tot_tri = tottri;
 
 	return buffers;
 }
 
+void GPU_update_grid_buffers(void *buffers_v, DMGridData **grids,
+	int *grid_indices, int totgrid, int gridsize)
+{
+	GPU_Buffers *buffers = buffers_v;
+	DMGridData *vert_data;
+	int i, totvert;
+
+	totvert= gridsize*gridsize*totgrid;
+
+	/* Build VBO */
+	glBindBufferARB(GL_ARRAY_BUFFER_ARB, buffers->vert_buf);
+	glBufferDataARB(GL_ARRAY_BUFFER_ARB,
+		     sizeof(DMGridData) * totvert,
+		     NULL, GL_STATIC_DRAW_ARB);
+	vert_data = glMapBufferARB(GL_ARRAY_BUFFER_ARB, GL_WRITE_ONLY_ARB);
+
+	for(i = 0; i < totgrid; ++i) {
+		DMGridData *grid= grids[grid_indices[i]];
+		memcpy(vert_data, grid, sizeof(DMGridData)*gridsize*gridsize);
+		vert_data += gridsize*gridsize;
+	}
+	glUnmapBufferARB(GL_ARRAY_BUFFER_ARB);
+
+	//printf("node updated %p\n", buffers_v);
+}
+
+void *GPU_build_grid_buffers(DMGridData **grids,
+	int *grid_indices, int totgrid, int gridsize)
+{
+	GPU_Buffers *buffers;
+	int i, j, k, totquad, offset= 0;
+
+	buffers = MEM_callocN(sizeof(GPU_Buffers), "GPU_Buffers");
+
+	/* Count the number of quads */
+	totquad= (gridsize-1)*(gridsize-1)*totgrid;
+
+	/* Generate index buffer object */
+	glGenBuffersARB(1, &buffers->index_buf);
+	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffers->index_buf);
+
+	if(totquad < USHRT_MAX) {
+		unsigned short *quad_data;
+
+		buffers->index_type = GL_UNSIGNED_SHORT;
+		glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB,
+				 sizeof(unsigned short) * totquad * 4, NULL, GL_STATIC_DRAW_ARB);
+
+		/* Fill the quad buffer */
+		quad_data = glMapBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, GL_WRITE_ONLY_ARB);
+		for(i = 0; i < totgrid; ++i) {
+			for(j = 0; j < gridsize-1; ++j) {
+				for(k = 0; k < gridsize-1; ++k) {
+					*(quad_data++)= offset + j*gridsize + k;
+					*(quad_data++)= offset + (j+1)*gridsize + k;
+					*(quad_data++)= offset + (j+1)*gridsize + k+1;
+					*(quad_data++)= offset + j*gridsize + k+1;
+				}
+			}
+
+			offset += gridsize*gridsize;
+		}
+		glUnmapBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB);
+	}
+	else {
+		unsigned int *quad_data;
+
+		buffers->index_type = GL_UNSIGNED_INT;
+		glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB,
+				 sizeof(unsigned int) * totquad * 4, NULL, GL_STATIC_DRAW_ARB);
+
+		/* Fill the quad buffer */
+		quad_data = glMapBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, GL_WRITE_ONLY_ARB);
+		for(i = 0; i < totgrid; ++i) {
+			for(j = 0; j < gridsize-1; ++j) {
+				for(k = 0; k < gridsize-1; ++k) {
+					*(quad_data++)= offset + j*gridsize + k;
+					*(quad_data++)= offset + (j+1)*gridsize + k;
+					*(quad_data++)= offset + (j+1)*gridsize + k+1;
+					*(quad_data++)= offset + j*gridsize + k+1;
+				}
+			}
+
+			offset += gridsize*gridsize;
+		}
+		glUnmapBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB);
+	}
+
+	/* Build VBO */
+	glGenBuffersARB(1, &buffers->vert_buf);
+	GPU_update_grid_buffers(buffers, grids, grid_indices, totgrid, gridsize);
+
+	buffers->tot_quad = totquad;
+
+	return buffers;
+}
+
 void GPU_draw_buffers(void *buffers_v)
 {
 	GPU_Buffers *buffers = buffers_v;
 
 	glBindBufferARB(GL_ARRAY_BUFFER_ARB, buffers->vert_buf);
-	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffers->tri_buf);
+	glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffers->index_buf);
+
+	if(buffers->tot_quad) {
+		glVertexPointer(3, GL_FLOAT, sizeof(DMGridData), 0);
+		glNormalPointer(GL_FLOAT, sizeof(DMGridData), (void*)offsetof(DMGridData, no));
 
-	glVertexPointer(3, GL_FLOAT, sizeof(VertexBufferFormat), 0);
-	glNormalPointer(GL_SHORT, sizeof(VertexBufferFormat), (void*)12);
+		glDrawElements(GL_QUADS, buffers->tot_quad * 4, buffers->index_type, 0);
+	}
+	else {
+		glVertexPointer(3, GL_FLOAT, sizeof(VertexBufferFormat), 0);
+		glNormalPointer(GL_SHORT, sizeof(VertexBufferFormat), (void*)offsetof(VertexBufferFormat, no));
 
-	glDrawElements(GL_TRIANGLES, buffers->tot_tri * 3, GL_UNSIGNED_SHORT, 0);
+		glDrawElements(GL_TRIANGLES, buffers->tot_tri * 3, buffers->index_type, 0);
+	}
 }
 
 void GPU_free_buffers(void *buffers_v)
@@ -496,7 +602,7 @@ void GPU_free_buffers(void *buffers_v)
 		GPU_Buffers *buffers = buffers_v;
 		
 		glDeleteBuffersARB(1, &buffers->vert_buf);
-		glDeleteBuffersARB(1, &buffers->tri_buf);
+		glDeleteBuffersARB(1, &buffers->index_buf);
 
 		MEM_freeN(buffers);
 	}