Commit of selfcollisions using new kdop design. Should result in nice speedup.

4 files changed, 234 insertions, 34 deletions

diff --git a/source/blender/blenkernel/BKE_cloth.h b/source/blender/blenkernel/BKE_cloth.h
index f01ed6bbea4..6575b8b873b 100644
--- a/source/blender/blenkernel/BKE_cloth.h
+++ b/source/blender/blenkernel/BKE_cloth.h
@@ -106,7 +106,7 @@ typedef struct Cloth
 	unsigned char 		pad2;
 	short 			pad3;
 	struct BVHTree		*bvhtree;			/* collision tree for this cloth object */
-	struct RayTree 		*selftree;			/* collision tree for this cloth object */
+	struct BVHTree 		*bvhselftree;			/* collision tree for this cloth object */
 	struct MFace 		*mfaces;
 	struct Implicit_Data	*implicit; 		/* our implicit solver connects to this pointer */
 	struct Implicit_Data	*implicitEM; 		/* our implicit solver connects to this pointer */
@@ -245,6 +245,7 @@ void cloth_update_normals ( ClothVertex *verts, int nVerts, MFace *face, int tot
 
 // needed for collision.c
 void bvhtree_update_from_cloth ( ClothModifierData *clmd, int moving );
+void bvhselftree_update_from_cloth ( ClothModifierData *clmd, int moving );
 
 // needed for editmesh.c
 void cloth_write_cache ( Object *ob, ClothModifierData *clmd, float framenr );
diff --git a/source/blender/blenkernel/intern/cloth.c b/source/blender/blenkernel/intern/cloth.c
index 6eb9f731056..916e3d6d3e8 100644
--- a/source/blender/blenkernel/intern/cloth.c
+++ b/source/blender/blenkernel/intern/cloth.c
@@ -189,6 +189,47 @@ void cloth_init ( ClothModifierData *clmd )
 	clmd->sim_parms->goalfrict = 0.0f;
 }
 
+BVHTree *bvhselftree_build_from_cloth (ClothModifierData *clmd, float epsilon)
+{
+	int i;
+	BVHTree *bvhtree;
+	Cloth *cloth = clmd->clothObject;
+	ClothVertex *verts;
+	MFace *mfaces;
+	float co[12];
+
+	if(!clmd)
+		return NULL;
+
+	cloth = clmd->clothObject;
+
+	if(!cloth)
+		return NULL;
+	
+	verts = cloth->verts;
+	mfaces = cloth->mfaces;
+	
+	// in the moment, return zero if no faces there
+	if(!cloth->numfaces)
+		return NULL;
+	
+	// create quadtree with k=26
+	bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 4, 6);
+	
+	// fill tree
+	for(i = 0; i < cloth->numverts; i++, verts++)
+	{
+		VECCOPY(&co[0*3], verts->xold);
+		
+		BLI_bvhtree_insert(bvhtree, i, co, 1);
+	}
+	
+	// balance tree
+	BLI_bvhtree_balance(bvhtree);
+	
+	return bvhtree;
+}
+
 BVHTree *bvhtree_build_from_cloth (ClothModifierData *clmd, float epsilon)
 {
 	int i;
@@ -214,7 +255,7 @@ BVHTree *bvhtree_build_from_cloth (ClothModifierData *clmd, float epsilon)
 		return NULL;
 	
 	// create quadtree with k=26
-	bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 8, 6);
+	bvhtree = BLI_bvhtree_new(cloth->numfaces, epsilon, 4, 26);
 	
 	// fill tree
 	for(i = 0; i < cloth->numfaces; i++, mfaces++)
@@ -289,6 +330,50 @@ void bvhtree_update_from_cloth(ClothModifierData *clmd, int moving)
 	}
 }
 
+void bvhselftree_update_from_cloth(ClothModifierData *clmd, int moving)
+{	
+	unsigned int i = 0;
+	Cloth *cloth = clmd->clothObject;
+	BVHTree *bvhtree = cloth->bvhselftree;
+	ClothVertex *verts = cloth->verts;
+	MFace *mfaces;
+	float co[12], co_moving[12];
+	int ret = 0;
+	
+	if(!bvhtree)
+		return;
+	
+	mfaces = cloth->mfaces;
+	
+	// update vertex position in bvh tree
+	if(verts && mfaces)
+	{
+		for(i = 0; i < cloth->numverts; i++, verts++)
+		{
+			VECCOPY(&co[0*3], verts->txold);
+			
+			// copy new locations into array
+			if(moving)
+			{
+				// update moving positions
+				VECCOPY(&co_moving[0*3], verts->tx);
+				
+				ret = BLI_bvhtree_update_node(bvhtree, i, co, co_moving, 1);
+			}
+			else
+			{
+				ret = BLI_bvhtree_update_node(bvhtree, i, co, NULL, 1);
+			}
+			
+			// check if tree is already full
+			if(!ret)
+				break;
+		}
+		
+		BLI_bvhtree_update_tree(bvhtree);
+	}
+}
+
 int modifiers_indexInObject(Object *ob, ModifierData *md_seek);
 
 int cloth_read_cache(Object *ob, ClothModifierData *clmd, float framenr)
@@ -599,6 +684,9 @@ void cloth_free_modifier ( Object *ob, ClothModifierData *clmd )
 		// free BVH collision tree
 		if ( cloth->bvhtree )
 			BLI_bvhtree_free ( cloth->bvhtree );
+		
+		if ( cloth->bvhselftree )
+			BLI_bvhtree_free ( cloth->bvhselftree );
 
 		// we save our faces for collision objects
 		if ( cloth->mfaces )
@@ -669,6 +757,9 @@ void cloth_free_modifier_extern ( ClothModifierData *clmd )
 		// free BVH collision tree
 		if ( cloth->bvhtree )
 			BLI_bvhtree_free ( cloth->bvhtree );
+		
+		if ( cloth->bvhselftree )
+			BLI_bvhtree_free ( cloth->bvhselftree );
 
 		// we save our faces for collision objects
 		if ( cloth->mfaces )
@@ -807,6 +898,7 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d
 	ClothVertex *verts = NULL;
 	float tnull[3] = {0,0,0};
 	Cloth *cloth = NULL;
+	float maxdist = 0;
 
 	// If we have a clothObject, free it. 
 	if ( clmd->clothObject != NULL )
@@ -876,7 +968,7 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d
 	// apply / set vertex groups
 	// has to be happen before springs are build!
 	cloth_apply_vgroup (clmd, dm);
-	
+
 	
 	if ( !cloth_build_springs ( clmd, dm ) )
 	{
@@ -904,6 +996,13 @@ static int cloth_from_object(Object *ob, ClothModifierData *clmd, DerivedMesh *d
 
 	BENCH(clmd->clothObject->bvhtree = bvhtree_build_from_cloth ( clmd, clmd->coll_parms->epsilon ));
 	
+	for(i = 0; i < dm->getNumVerts(dm); i++)
+	{
+		maxdist = MAX2(maxdist, clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len*2.0));
+	}
+	
+	clmd->clothObject->bvhselftree = bvhselftree_build_from_cloth ( clmd, maxdist );
+
 	return 1;
 }
 
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c
index 1e8b8706658..4d49ccc9eb3 100644
--- a/source/blender/blenkernel/intern/collision.c
+++ b/source/blender/blenkernel/intern/collision.c
@@ -1260,7 +1260,7 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
 	Cloth *cloth=NULL;
 	Object *coll_ob=NULL;
 	BVHTree *cloth_bvh=NULL;
-	long i=0, j = 0, numfaces = 0, numverts = 0;
+	long i=0, j = 0, k = 0, numfaces = 0, numverts = 0;
 	unsigned int result = 0, rounds = 0; // result counts applied collisions; ic is for debug output;
 	ClothVertex *verts = NULL;
 	int ret = 0;
@@ -1284,6 +1284,7 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
 
 	// update cloth bvh
 	bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
+	bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
 
 	do
 	{
@@ -1357,12 +1358,82 @@ int cloth_bvh_objcollision ( ClothModifierData * clmd, float step, float dt )
 		if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
 		{
 
-			MFace *mface = clmd->clothObject->mfaces;
+			MFace *mface = cloth->mfaces;
+			BVHTreeOverlap *overlap = NULL;
 
 			collisions = 1;
 			verts = cloth->verts; // needed for openMP
 
+			numfaces = clmd->clothObject->numfaces;
+			numverts = clmd->clothObject->numverts;
+
+			verts = cloth->verts;
+
+			if ( cloth->bvhselftree )
+			{
+				/* search for overlapping collision pairs */
+				overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
+
+				for ( k = 0; k < result; k++ )
+				{
+					float temp[3];
+					float length = 0;
+					float mindistance;
+					
+					i = overlap[k].indexA;
+					j = overlap[k].indexB;
+					
+					mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
+
+					if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
+					{
+						if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+												 && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
+						{
+							continue;
+						}
+					}
+
+					VECSUB ( temp, verts[i].tx, verts[j].tx );
+
+					if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
+
+					// check for adjacent points (i must be smaller j)
+					if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
+					{
+						continue;
+					}
 
+					length = Normalize ( temp );
+
+					if ( length < mindistance )
+					{
+						float correction = mindistance - length;
+
+						if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
+						{
+							VecMulf ( temp, -correction );
+							VECADD ( verts[j].tx, verts[j].tx, temp );
+						}
+						else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
+						{
+							VecMulf ( temp, correction );
+							VECADD ( verts[i].tx, verts[i].tx, temp );
+						}
+						else
+						{
+							VecMulf ( temp, -correction*0.5 );
+							VECADD ( verts[j].tx, verts[j].tx, temp );
+
+							VECSUB ( verts[i].tx, verts[i].tx, temp );
+						}
+					}
+				}
+				
+				if ( overlap )
+					MEM_freeN ( overlap );
+				
+			}
 
 			/*
 			for ( count = 0; count < clmd->coll_parms->self_loop_count; count++ )
diff --git a/source/blender/blenlib/intern/BLI_kdopbvh.c b/source/blender/blenlib/intern/BLI_kdopbvh.c
index b3f11039ce1..83afe258aad 100644
--- a/source/blender/blenlib/intern/BLI_kdopbvh.c
+++ b/source/blender/blenlib/intern/BLI_kdopbvh.c
@@ -80,12 +80,11 @@
 #endif
 
 
-
 typedef struct BVHNode
 {
-	struct BVHNode *children[8]; // max 8 children
+	struct BVHNode **children; // max 8 children
 	struct BVHNode *parent; // needed for bottom - top update
-	float bv[26]; // Bounding volume of all nodes, max 13 axis
+	float *bv; // Bounding volume of all nodes, max 13 axis
 	int index; /* face, edge, vertex index */
 	char totnode; // how many nodes are used, used for speedup
 	char traversed;  // how many nodes already traversed until this level?
@@ -96,7 +95,9 @@ struct BVHTree
 {
 	BVHNode **nodes;
 	BVHNode *nodearray; /* pre-alloc branch nodes */
-	float 	epsilon; /* epsilon is used for inflation of the k-dop	   */
+	BVHNode **nodechild;	// pre-alloc childs for nodes
+	float	*nodebv;		// pre-alloc bounding-volumes for nodes
+	float 	epsilon; /* epslion is used for inflation of the k-dop	   */
 	int 	totleaf; // leafs
 	int 	totbranch;
 	char 	tree_type; // type of tree (4 => quadtree)
@@ -301,6 +302,8 @@ void BLI_bvhtree_free(BVHTree *tree)
 	{
 		MEM_freeN(tree->nodes);
 		MEM_freeN(tree->nodearray);
+		MEM_freeN(tree->nodebv);
+		MEM_freeN(tree->nodechild);
 		MEM_freeN(tree);
 	}
 }
@@ -318,27 +321,6 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
 	
 	if(tree)
 	{
-		// calculate max number of branches, our bvh kdop is "almost perfect"
-		for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++)
-			numbranches += (pow(tree_type, i) / tree_type);
-		
-		tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
-		
-		if(!tree->nodes)
-		{
-			MEM_freeN(tree);
-			return NULL;
-		}
-		
-		tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
-		
-		if(!tree->nodearray)
-		{
-			MEM_freeN(tree);
-			MEM_freeN(tree->nodes);
-			return NULL;
-		}
-		
 		tree->epsilon = epsilon;
 		tree->tree_type = tree_type; 
 		tree->axis = axis;
@@ -370,14 +352,62 @@ BVHTree *BLI_bvhtree_new(int maxsize, float epsilon, char tree_type, char axis)
 		}
 		else
 		{
-			BLI_bvhtree_free(tree);
+			MEM_freeN(tree);
 			return NULL;
 		}
+
+
+		// calculate max number of branches, our bvh kdop is "almost perfect"
+		for(i = 1; i <= (int)ceil((float)((float)log(maxsize)/(float)log(tree_type))); i++)
+			numbranches += (pow(tree_type, i) / tree_type);
+		
+		tree->nodes = (BVHNode **)MEM_callocN(sizeof(BVHNode *)*(numbranches+maxsize + tree_type), "BVHNodes");
+		
+		if(!tree->nodes)
+		{
+			MEM_freeN(tree);
+			return NULL;
+		}
+		
+		tree->nodebv = (float*)MEM_callocN(sizeof(float)* axis * (numbranches+maxsize + tree_type), "BVHNodeBV");
+		if(!tree->nodebv)
+		{
+			MEM_freeN(tree->nodes);
+			MEM_freeN(tree);
+		}
+
+		tree->nodechild = (BVHNode**)MEM_callocN(sizeof(BVHNode*) * tree_type * (numbranches+maxsize + tree_type), "BVHNodeBV");
+		if(!tree->nodechild)
+		{
+			MEM_freeN(tree->nodebv);
+			MEM_freeN(tree->nodes);
+			MEM_freeN(tree);
+		}
+
+		tree->nodearray = (BVHNode *)MEM_callocN(sizeof(BVHNode)*(numbranches+maxsize + tree_type), "BVHNodeArray");
+		
+		if(!tree->nodearray)
+		{
+			MEM_freeN(tree->nodechild);
+			MEM_freeN(tree->nodebv);
+			MEM_freeN(tree->nodes);
+			MEM_freeN(tree);
+			return NULL;
+		}
+
+		//link the dynamic bv and child links
+		for(i=0; i< numbranches+maxsize + tree_type; i++)
+		{
+			tree->nodearray[i].bv = tree->nodebv + i * axis;
+			tree->nodearray[i].children = tree->nodechild + i * tree_type;
+		}
+		
 	}
 
 	return tree;
 }
 
+
 static void create_kdop_hull(BVHTree *tree, BVHNode *node, float *co, int numpoints, int moving)
 {
 	float newminmax;
@@ -507,8 +537,6 @@ static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char
 	{	
 		tend = start + slice;
 		
-		partition_nth_element(tree->nodes, start, end, tend, laxis);
-		
 		if(tend > end) tend = end;
 		
 		if(tend-start == 1)	// ok, we have 1 left for this node
@@ -521,7 +549,8 @@ static void bvh_div_nodes(BVHTree *tree, BVHNode *node, int start, int end, char
 			tnode = node->children[i] = tree->nodes[tree->totleaf  + tree->totbranch] = &(tree->nodearray[tree->totbranch + tree->totleaf]);
 			tree->totbranch++;
 			tnode->parent = node;
-
+			
+			partition_nth_element(tree->nodes, start, end, tend, laxis);
 			refit_kdop_hull(tree, tnode, start, tend);
 			bvh_div_nodes(tree, tnode, start, tend, laxis);
 		}