Replace MFace w/ vert-tri's for collision modifier

Note that the collision modifier doesn't have any use for Loop indices,
so to avoid duplicating the loop array too,
MVertTri has been added which simply stores vertex indices (runtime only).

1 files changed, 186 insertions, 409 deletions

diff --git a/source/blender/blenkernel/intern/softbody.c b/source/blender/blenkernel/intern/softbody.c
index 055c3a86d0e..0044c8df298 100644
--- a/source/blender/blenkernel/intern/softbody.c
+++ b/source/blender/blenkernel/intern/softbody.c
@@ -171,8 +171,6 @@ static float SoftHeunTol = 1.0f; /* humm .. this should be calculated from sb pa
 
 /* local prototypes */
 static void free_softbody_intern(SoftBody *sb);
-/* aye this belongs to arith.c */
-static void Vec3PlusStVec(float *v, float s, float *v1);
 
 /*+++ frame based timing +++*/
 
@@ -276,10 +274,10 @@ typedef struct ccdf_minmax {
 
 
 typedef struct ccd_Mesh {
-	int totvert, totface;
-	MVert *mvert;
-	MVert *mprevvert;
-	MFace *mface;
+	int mvert_num, tri_num;
+	const MVert *mvert;
+	const MVert *mprevvert;
+	const MVertTri *tri;
 	int savety;
 	ccdf_minmax *mima;
 	/* Axis Aligned Bounding Box AABB */
@@ -294,26 +292,25 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 {
 	CollisionModifierData *cmd;
 	ccd_Mesh *pccd_M = NULL;
-	ccdf_minmax *mima = NULL;
-	MFace *mface=NULL;
-	float v[3], hull;
+	ccdf_minmax *mima;
+	const MVertTri *vt;
+	float hull;
 	int i;
 
 	cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
 
 	/* first some paranoia checks */
 	if (!cmd) return NULL;
-	if (!cmd->numverts || !cmd->numfaces) return NULL;
+	if (!cmd->mvert_num || !cmd->tri_num) return NULL;
 
 	pccd_M = MEM_mallocN(sizeof(ccd_Mesh), "ccd_Mesh");
-	pccd_M->totvert = cmd->numverts;
-	pccd_M->totface = cmd->numfaces;
+	pccd_M->mvert_num = cmd->mvert_num;
+	pccd_M->tri_num = cmd->tri_num;
 	pccd_M->savety  = CCD_SAVETY;
 	pccd_M->bbmin[0]=pccd_M->bbmin[1]=pccd_M->bbmin[2]=1e30f;
 	pccd_M->bbmax[0]=pccd_M->bbmax[1]=pccd_M->bbmax[2]=-1e30f;
 	pccd_M->mprevvert=NULL;
 
-
 	/* blow it up with forcefield ranges */
 	hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
 
@@ -321,9 +318,11 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 	pccd_M->mvert = MEM_dupallocN(cmd->xnew);
 	/* note that xnew coords are already in global space, */
 	/* determine the ortho BB */
-	for (i=0; i < pccd_M->totvert; i++) {
+	for (i = 0; i < pccd_M->mvert_num; i++) {
+		const float *v;
+
 		/* evaluate limits */
-		copy_v3_v3(v, pccd_M->mvert[i].co);
+		v = pccd_M->mvert[i].co;
 		pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
 		pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
 		pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
@@ -334,20 +333,20 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 
 	}
 	/* alloc and copy faces*/
-	pccd_M->mface = MEM_dupallocN(cmd->mfaces);
+	pccd_M->tri = MEM_dupallocN(cmd->tri);
 
 	/* OBBs for idea1 */
-	pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax)*pccd_M->totface, "ccd_Mesh_Faces_mima");
-	mima  = pccd_M->mima;
-	mface = pccd_M->mface;
+	pccd_M->mima = MEM_mallocN(sizeof(ccdf_minmax) * pccd_M->tri_num, "ccd_Mesh_Faces_mima");
 
 
 	/* anyhoo we need to walk the list of faces and find OBB they live in */
-	for (i=0; i < pccd_M->totface; i++) {
+	for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
+		const float *v;
+
 		mima->minx=mima->miny=mima->minz=1e30f;
 		mima->maxx=mima->maxy=mima->maxz=-1e30f;
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v1].co);
+		v = pccd_M->mvert[vt->tri[0]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -355,7 +354,7 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v2].co);
+		v = pccd_M->mvert[vt->tri[1]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -363,26 +362,13 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v3].co);
+		v = pccd_M->mvert[vt->tri[2]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
 		mima->maxx = max_ff(mima->maxx, v[0] + hull);
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
-
-		if (mface->v4) {
-			copy_v3_v3(v, pccd_M->mvert[mface->v4].co);
-			mima->minx = min_ff(mima->minx, v[0] - hull);
-			mima->miny = min_ff(mima->miny, v[1] - hull);
-			mima->minz = min_ff(mima->minz, v[2] - hull);
-			mima->maxx = max_ff(mima->maxx, v[0] + hull);
-			mima->maxy = max_ff(mima->maxy, v[1] + hull);
-			mima->maxz = max_ff(mima->maxz, v[2] + hull);
-		}
-
-		mima++;
-		mface++;
 	}
 
 	return pccd_M;
@@ -390,19 +376,19 @@ static ccd_Mesh *ccd_mesh_make(Object *ob)
 static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 {
 	CollisionModifierData *cmd;
-	ccdf_minmax *mima = NULL;
-	MFace *mface=NULL;
-	float v[3], hull;
+	ccdf_minmax *mima;
+	const MVertTri *vt;
+	float hull;
 	int i;
 
 	cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
 
 	/* first some paranoia checks */
 	if (!cmd) return;
-	if (!cmd->numverts || !cmd->numfaces) return;
+	if (!cmd->mvert_num || !cmd->tri_num) return;
 
-	if ((pccd_M->totvert != cmd->numverts) ||
-	    (pccd_M->totface != cmd->numfaces))
+	if ((pccd_M->mvert_num != cmd->mvert_num) ||
+	    (pccd_M->tri_num != cmd->tri_num))
 	{
 		return;
 	}
@@ -415,15 +401,17 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 	hull = max_ff(ob->pd->pdef_sbift, ob->pd->pdef_sboft);
 
 	/* rotate current to previous */
-	if (pccd_M->mprevvert) MEM_freeN(pccd_M->mprevvert);
+	if (pccd_M->mprevvert) MEM_freeN((void *)pccd_M->mprevvert);
 	pccd_M->mprevvert = pccd_M->mvert;
 	/* alloc and copy verts*/
 	pccd_M->mvert = MEM_dupallocN(cmd->xnew);
 	/* note that xnew coords are already in global space, */
 	/* determine the ortho BB */
-	for (i=0; i < pccd_M->totvert; i++) {
+	for (i=0; i < pccd_M->mvert_num; i++) {
+		const float *v;
+
 		/* evaluate limits */
-		copy_v3_v3(v, pccd_M->mvert[i].co);
+		v = pccd_M->mvert[i].co;
 		pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
 		pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
 		pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
@@ -433,7 +421,7 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		pccd_M->bbmax[2] = max_ff(pccd_M->bbmax[2], v[2] + hull);
 
 		/* evaluate limits */
-		copy_v3_v3(v, pccd_M->mprevvert[i].co);
+		v = pccd_M->mprevvert[i].co;
 		pccd_M->bbmin[0] = min_ff(pccd_M->bbmin[0], v[0] - hull);
 		pccd_M->bbmin[1] = min_ff(pccd_M->bbmin[1], v[1] - hull);
 		pccd_M->bbmin[2] = min_ff(pccd_M->bbmin[2], v[2] - hull);
@@ -444,16 +432,15 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 
 	}
 
-	mima  = pccd_M->mima;
-	mface = pccd_M->mface;
-
-
 	/* anyhoo we need to walk the list of faces and find OBB they live in */
-	for (i=0; i < pccd_M->totface; i++) {
+	for (i = 0, mima  = pccd_M->mima, vt = pccd_M->tri; i < pccd_M->tri_num; i++, mima++, vt++) {
+		const float *v;
+
 		mima->minx=mima->miny=mima->minz=1e30f;
 		mima->maxx=mima->maxy=mima->maxz=-1e30f;
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v1].co);
+		/* mvert */
+		v = pccd_M->mvert[vt->tri[0]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -461,7 +448,7 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v2].co);
+		v = pccd_M->mvert[vt->tri[1]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -469,7 +456,7 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mvert[mface->v3].co);
+		v = pccd_M->mvert[vt->tri[2]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -477,18 +464,9 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		if (mface->v4) {
-			copy_v3_v3(v, pccd_M->mvert[mface->v4].co);
-			mima->minx = min_ff(mima->minx, v[0] - hull);
-			mima->miny = min_ff(mima->miny, v[1] - hull);
-			mima->minz = min_ff(mima->minz, v[2] - hull);
-			mima->maxx = max_ff(mima->maxx, v[0] + hull);
-			mima->maxy = max_ff(mima->maxy, v[1] + hull);
-			mima->maxz = max_ff(mima->maxz, v[2] + hull);
-		}
-
 
-		copy_v3_v3(v, pccd_M->mprevvert[mface->v1].co);
+		/* mprevvert */
+		v = pccd_M->mprevvert[vt->tri[0]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -496,7 +474,7 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mprevvert[mface->v2].co);
+		v = pccd_M->mprevvert[vt->tri[1]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
@@ -504,26 +482,13 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
 
-		copy_v3_v3(v, pccd_M->mprevvert[mface->v3].co);
+		v = pccd_M->mprevvert[vt->tri[2]].co;
 		mima->minx = min_ff(mima->minx, v[0] - hull);
 		mima->miny = min_ff(mima->miny, v[1] - hull);
 		mima->minz = min_ff(mima->minz, v[2] - hull);
 		mima->maxx = max_ff(mima->maxx, v[0] + hull);
 		mima->maxy = max_ff(mima->maxy, v[1] + hull);
 		mima->maxz = max_ff(mima->maxz, v[2] + hull);
-
-		if (mface->v4) {
-			copy_v3_v3(v, pccd_M->mprevvert[mface->v4].co);
-			mima->minx = min_ff(mima->minx, v[0] - hull);
-			mima->miny = min_ff(mima->miny, v[1] - hull);
-			mima->minz = min_ff(mima->minz, v[2] - hull);
-			mima->maxx = max_ff(mima->maxx, v[0] + hull);
-			mima->maxy = max_ff(mima->maxy, v[1] + hull);
-			mima->maxz = max_ff(mima->maxz, v[2] + hull);
-		}
-
-		mima++;
-		mface++;
 	}
 	return;
 }
@@ -531,9 +496,9 @@ static void ccd_mesh_update(Object *ob, ccd_Mesh *pccd_M)
 static void ccd_mesh_free(ccd_Mesh *ccdm)
 {
 	if (ccdm && (ccdm->savety == CCD_SAVETY )) { /*make sure we're not nuking objects we don't know*/
-		MEM_freeN(ccdm->mface);
-		MEM_freeN(ccdm->mvert);
-		if (ccdm->mprevvert) MEM_freeN(ccdm->mprevvert);
+		MEM_freeN((void *)ccdm->mvert);
+		MEM_freeN((void *)ccdm->tri);
+		if (ccdm->mprevvert) MEM_freeN((void *)ccdm->mprevvert);
 		MEM_freeN(ccdm->mima);
 		MEM_freeN(ccdm);
 		ccdm = NULL;
@@ -982,14 +947,6 @@ static void free_softbody_intern(SoftBody *sb)
 ** since that would only valid for 'slow' moving collision targets and dito particles
 */
 
-/* aye this belongs to arith.c */
-static void Vec3PlusStVec(float *v, float s, float *v1)
-{
-	v[0] += s*v1[0];
-	v[1] += s*v1[1];
-	v[2] += s*v1[2];
-}
-
 /* +++ dependency information functions*/
 
 static int are_there_deflectors(Scene *scene, unsigned int layer)
@@ -1119,11 +1076,11 @@ static int sb_detect_face_pointCached(float face_v1[3], float face_v2[3], float
 		{
 			/* only with deflecting set */
 			if (ob->pd && ob->pd->deflect) {
-				MVert *mvert= NULL;
-				MVert *mprevvert= NULL;
+				const MVert *mvert= NULL;
+				const MVert *mprevvert= NULL;
 				if (ccdm) {
-					mvert= ccdm->mvert;
-					a    = ccdm->totvert;
+					mvert = ccdm->mvert;
+					a     = ccdm->mvert_num;
 					mprevvert= ccdm->mprevvert;
 					outerfacethickness = ob->pd->pdef_sboft;
 					if ((aabbmax[0] < ccdm->bbmin[0]) ||
@@ -1153,7 +1110,7 @@ static int sb_detect_face_pointCached(float face_v1[3], float face_v2[3], float
 						copy_v3_v3(nv1, mvert[a-1].co);
 						if (mprevvert) {
 							mul_v3_fl(nv1, time);
-							Vec3PlusStVec(nv1, (1.0f-time), mprevvert[a-1].co);
+							madd_v3_v3fl(nv1, mprevvert[a - 1].co, 1.0f - time);
 						}
 						/* origin to face_v2*/
 						sub_v3_v3(nv1, face_v2);
@@ -1171,7 +1128,7 @@ static int sb_detect_face_pointCached(float face_v1[3], float face_v2[3], float
 								*damp=df*tune*ob->pd->pdef_sbdamp;
 
 								df = 0.01f * expf(-100.0f * df);
-								Vec3PlusStVec(force, -df, d_nvect);
+								madd_v3_v3fl(force, d_nvect, -df);
 								deflected = 3;
 							}
 						}
@@ -1193,7 +1150,7 @@ static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], fl
 	Object *ob;
 	GHash *hash;
 	GHashIterator *ihash;
-	float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
+	float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
 	float t, tune = 10.0f;
 	int a, deflected=0;
 
@@ -1213,16 +1170,17 @@ static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], fl
 		{
 			/* only with deflecting set */
 			if (ob->pd && ob->pd->deflect) {
-				MFace *mface= NULL;
-				MVert *mvert= NULL;
-				MVert *mprevvert= NULL;
-				ccdf_minmax *mima= NULL;
+				const MVert *mvert = NULL;
+				const MVert *mprevvert = NULL;
+				const MVertTri *vt = NULL;
+				const ccdf_minmax *mima = NULL;
+
 				if (ccdm) {
-					mface= ccdm->mface;
-					mvert= ccdm->mvert;
-					mprevvert= ccdm->mprevvert;
-					mima= ccdm->mima;
-					a = ccdm->totface;
+					mvert = ccdm->mvert;
+					vt = ccdm->tri;
+					mprevvert = ccdm->mprevvert;
+					mima = ccdm->mima;
+					a = ccdm->tri_num;
 
 					if ((aabbmax[0] < ccdm->bbmin[0]) ||
 					    (aabbmax[1] < ccdm->bbmin[1]) ||
@@ -1247,42 +1205,34 @@ static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], fl
 
 				/* use mesh*/
 				while (a--) {
-					if (
-						(aabbmax[0] < mima->minx) ||
-						(aabbmin[0] > mima->maxx) ||
-						(aabbmax[1] < mima->miny) ||
-						(aabbmin[1] > mima->maxy) ||
-						(aabbmax[2] < mima->minz) ||
-						(aabbmin[2] > mima->maxz)
-						) {
-						mface++;
+					if ((aabbmax[0] < mima->minx) ||
+					    (aabbmin[0] > mima->maxx) ||
+					    (aabbmax[1] < mima->miny) ||
+					    (aabbmin[1] > mima->maxy) ||
+					    (aabbmax[2] < mima->minz) ||
+					    (aabbmin[2] > mima->maxz))
+					{
 						mima++;
+						vt++;
 						continue;
 					}
 
 
 					if (mvert) {
 
-						copy_v3_v3(nv1, mvert[mface->v1].co);
-						copy_v3_v3(nv2, mvert[mface->v2].co);
-						copy_v3_v3(nv3, mvert[mface->v3].co);
-						if (mface->v4) {
-							copy_v3_v3(nv4, mvert[mface->v4].co);
-						}
+						copy_v3_v3(nv1, mvert[vt->tri[0]].co);
+						copy_v3_v3(nv2, mvert[vt->tri[1]].co);
+						copy_v3_v3(nv3, mvert[vt->tri[2]].co);
+
 						if (mprevvert) {
 							mul_v3_fl(nv1, time);
-							Vec3PlusStVec(nv1, (1.0f-time), mprevvert[mface->v1].co);
+							madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
 
 							mul_v3_fl(nv2, time);
-							Vec3PlusStVec(nv2, (1.0f-time), mprevvert[mface->v2].co);
+							madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
 
 							mul_v3_fl(nv3, time);
-							Vec3PlusStVec(nv3, (1.0f-time), mprevvert[mface->v3].co);
-
-							if (mface->v4) {
-								mul_v3_fl(nv4, time);
-								Vec3PlusStVec(nv4, (1.0f-time), mprevvert[mface->v4].co);
-							}
+							madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
 						}
 					}
 
@@ -1291,32 +1241,16 @@ static int sb_detect_face_collisionCached(float face_v1[3], float face_v2[3], fl
 					sub_v3_v3v3(edge2, nv3, nv2);
 					cross_v3_v3v3(d_nvect, edge2, edge1);
 					normalize_v3(d_nvect);
-					if (
-						isect_line_tri_v3(nv1, nv2, face_v1, face_v2, face_v3, &t, NULL) ||
-						isect_line_tri_v3(nv2, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
-						isect_line_tri_v3(nv3, nv1, face_v1, face_v2, face_v3, &t, NULL) ) {
-						Vec3PlusStVec(force, -0.5f, d_nvect);
+					if (isect_line_tri_v3(nv1, nv2, face_v1, face_v2, face_v3, &t, NULL) ||
+					    isect_line_tri_v3(nv2, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
+					    isect_line_tri_v3(nv3, nv1, face_v1, face_v2, face_v3, &t, NULL) )
+					{
+						madd_v3_v3fl(force, d_nvect, -0.5f);
 						*damp=tune*ob->pd->pdef_sbdamp;
 						deflected = 2;
 					}
-					if (mface->v4) { /* quad */
-						/* switch origin to be nv4 */
-						sub_v3_v3v3(edge1, nv3, nv4);
-						sub_v3_v3v3(edge2, nv1, nv4);
-						cross_v3_v3v3(d_nvect, edge2, edge1);
-						normalize_v3(d_nvect);
-						if (
-							/* isect_line_tri_v3(nv1, nv3, face_v1, face_v2, face_v3, &t, NULL) ||
-							 * we did that edge already */
-							isect_line_tri_v3(nv3, nv4, face_v1, face_v2, face_v3, &t, NULL) ||
-							isect_line_tri_v3(nv4, nv1, face_v1, face_v2, face_v3, &t, NULL) ) {
-							Vec3PlusStVec(force, -0.5f, d_nvect);
-							*damp=tune*ob->pd->pdef_sbdamp;
-							deflected = 2;
-						}
-					}
-					mface++;
 					mima++;
+					vt++;
 				}/* while a */
 			} /* if (ob->pd && ob->pd->deflect) */
 			BLI_ghashIterator_step(ihash);
@@ -1345,26 +1279,15 @@ static void scan_for_ext_face_forces(Object *ob, float timenow)
 			bf->ext_force[0]=bf->ext_force[1]=bf->ext_force[2]=0.0f;
 /*+++edges intruding*/
 			bf->flag &= ~BFF_INTERSECT;
-			feedback[0]=feedback[1]=feedback[2]=0.0f;
-			if (sb_detect_face_collisionCached(sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
-			                                   &damp, feedback, ob->lay, ob, timenow))
+			zero_v3(feedback);
+			if (sb_detect_face_collisionCached(
+			        sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
+			        &damp, feedback, ob->lay, ob, timenow))
 			{
-				Vec3PlusStVec(sb->bpoint[bf->v1].force, tune, feedback);
-				Vec3PlusStVec(sb->bpoint[bf->v2].force, tune, feedback);
-				Vec3PlusStVec(sb->bpoint[bf->v3].force, tune, feedback);
-//				Vec3PlusStVec(bf->ext_force, tune, feedback);
-				bf->flag |= BFF_INTERSECT;
-				choke = min_ff(max_ff(damp, choke), 1.0f);
-			}
-
-			feedback[0]=feedback[1]=feedback[2]=0.0f;
-			if ((bf->v4) && (sb_detect_face_collisionCached(sb->bpoint[bf->v1].pos, sb->bpoint[bf->v3].pos, sb->bpoint[bf->v4].pos,
-			                                                &damp, feedback, ob->lay, ob, timenow)))
-			{
-				Vec3PlusStVec(sb->bpoint[bf->v1].force, tune, feedback);
-				Vec3PlusStVec(sb->bpoint[bf->v3].force, tune, feedback);
-				Vec3PlusStVec(sb->bpoint[bf->v4].force, tune, feedback);
-//				Vec3PlusStVec(bf->ext_force, tune, feedback);
+				madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
+				madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
+				madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
+//				madd_v3_v3fl(bf->ext_force, feedback, tune);
 				bf->flag |= BFF_INTERSECT;
 				choke = min_ff(max_ff(damp, choke), 1.0f);
 			}
@@ -1374,26 +1297,15 @@ static void scan_for_ext_face_forces(Object *ob, float timenow)
 			if (( bf->flag & BFF_INTERSECT)==0) {
 				bf->flag &= ~BFF_CLOSEVERT;
 				tune = -1.0f;
-				feedback[0]=feedback[1]=feedback[2]=0.0f;
-				if (sb_detect_face_pointCached(sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
-					&damp,	feedback, ob->lay, ob, timenow))
-				{
-					Vec3PlusStVec(sb->bpoint[bf->v1].force, tune, feedback);
-					Vec3PlusStVec(sb->bpoint[bf->v2].force, tune, feedback);
-					Vec3PlusStVec(sb->bpoint[bf->v3].force, tune, feedback);
-//					Vec3PlusStVec(bf->ext_force, tune, feedback);
-					bf->flag |= BFF_CLOSEVERT;
-					choke = min_ff(max_ff(damp, choke), 1.0f);
-				}
-
-				feedback[0]=feedback[1]=feedback[2]=0.0f;
-				if ((bf->v4) && (sb_detect_face_pointCached(sb->bpoint[bf->v1].pos, sb->bpoint[bf->v3].pos, sb->bpoint[bf->v4].pos,
-				                                            &damp, feedback, ob->lay, ob, timenow)))
+				zero_v3(feedback);
+				if (sb_detect_face_pointCached(
+				        sb->bpoint[bf->v1].pos, sb->bpoint[bf->v2].pos, sb->bpoint[bf->v3].pos,
+				        &damp,	feedback, ob->lay, ob, timenow))
 				{
-					Vec3PlusStVec(sb->bpoint[bf->v1].force, tune, feedback);
-					Vec3PlusStVec(sb->bpoint[bf->v3].force, tune, feedback);
-					Vec3PlusStVec(sb->bpoint[bf->v4].force, tune, feedback);
-//					Vec3PlusStVec(bf->ext_force, tune, feedback);
+					madd_v3_v3fl(sb->bpoint[bf->v1].force, feedback, tune);
+					madd_v3_v3fl(sb->bpoint[bf->v2].force, feedback, tune);
+					madd_v3_v3fl(sb->bpoint[bf->v3].force, feedback, tune);
+//					madd_v3_v3fl(bf->ext_force, feedback, tune);
 					bf->flag |= BFF_CLOSEVERT;
 					choke = min_ff(max_ff(damp, choke), 1.0f);
 				}
@@ -1406,9 +1318,6 @@ static void scan_for_ext_face_forces(Object *ob, float timenow)
 				sb->bpoint[bf->v1].choke2 = max_ff(sb->bpoint[bf->v1].choke2, choke);
 				sb->bpoint[bf->v2].choke2 = max_ff(sb->bpoint[bf->v2].choke2, choke);
 				sb->bpoint[bf->v3].choke2 = max_ff(sb->bpoint[bf->v3].choke2, choke);
-				if (bf->v4) {
-					sb->bpoint[bf->v2].choke2 = max_ff(sb->bpoint[bf->v2].choke2, choke);
-				}
 			}
 		}
 	}
@@ -1425,7 +1334,7 @@ static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], fl
 	Object *ob;
 	GHash *hash;
 	GHashIterator *ihash;
-	float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
+	float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], aabbmin[3], aabbmax[3];
 	float t, el;
 	int a, deflected=0;
 
@@ -1443,16 +1352,17 @@ static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], fl
 		{
 			/* only with deflecting set */
 			if (ob->pd && ob->pd->deflect) {
-				MFace *mface= NULL;
-				MVert *mvert= NULL;
-				MVert *mprevvert= NULL;
-				ccdf_minmax *mima= NULL;
+				const MVert *mvert = NULL;
+				const MVert *mprevvert = NULL;
+				const MVertTri *vt = NULL;
+				const ccdf_minmax *mima = NULL;
+
 				if (ccdm) {
-					mface= ccdm->mface;
-					mvert= ccdm->mvert;
-					mprevvert= ccdm->mprevvert;
-					mima= ccdm->mima;
-					a = ccdm->totface;
+					mvert = ccdm->mvert;
+					mprevvert = ccdm->mprevvert;
+					vt = ccdm->tri;
+					mima = ccdm->mima;
+					a = ccdm->tri_num;
 
 					if ((aabbmax[0] < ccdm->bbmin[0]) ||
 					    (aabbmax[1] < ccdm->bbmin[1]) ||
@@ -1477,43 +1387,34 @@ static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], fl
 
 				/* use mesh*/
 				while (a--) {
-					if (
-						(aabbmax[0] < mima->minx) ||
-						(aabbmin[0] > mima->maxx) ||
-						(aabbmax[1] < mima->miny) ||
-						(aabbmin[1] > mima->maxy) ||
-						(aabbmax[2] < mima->minz) ||
-						(aabbmin[2] > mima->maxz)
-						)
+					if ((aabbmax[0] < mima->minx) ||
+					    (aabbmin[0] > mima->maxx) ||
+					    (aabbmax[1] < mima->miny) ||
+					    (aabbmin[1] > mima->maxy) ||
+					    (aabbmax[2] < mima->minz) ||
+					    (aabbmin[2] > mima->maxz))
 					{
-						mface++;
 						mima++;
+						vt++;
 						continue;
 					}
 
 
 					if (mvert) {
 
-						copy_v3_v3(nv1, mvert[mface->v1].co);
-						copy_v3_v3(nv2, mvert[mface->v2].co);
-						copy_v3_v3(nv3, mvert[mface->v3].co);
-						if (mface->v4) {
-							copy_v3_v3(nv4, mvert[mface->v4].co);
-						}
+						copy_v3_v3(nv1, mvert[vt->tri[0]].co);
+						copy_v3_v3(nv2, mvert[vt->tri[1]].co);
+						copy_v3_v3(nv3, mvert[vt->tri[2]].co);
+
 						if (mprevvert) {
 							mul_v3_fl(nv1, time);
-							Vec3PlusStVec(nv1, (1.0f-time), mprevvert[mface->v1].co);
+							madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
 
 							mul_v3_fl(nv2, time);
-							Vec3PlusStVec(nv2, (1.0f-time), mprevvert[mface->v2].co);
+							madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
 
 							mul_v3_fl(nv3, time);
-							Vec3PlusStVec(nv3, (1.0f-time), mprevvert[mface->v3].co);
-
-							if (mface->v4) {
-								mul_v3_fl(nv4, time);
-								Vec3PlusStVec(nv4, (1.0f-time), mprevvert[mface->v4].co);
-							}
+							madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
 						}
 					}
 
@@ -1531,34 +1432,13 @@ static int sb_detect_edge_collisionCached(float edge_v1[3], float edge_v2[3], fl
 						i1 = dot_v3v3(v1, d_nvect);
 						i2 = dot_v3v3(v2, d_nvect);
 						intrusiondepth = -min_ff(i1, i2) / el;
-						Vec3PlusStVec(force, intrusiondepth, d_nvect);
+						madd_v3_v3fl(force, d_nvect, intrusiondepth);
 						*damp=ob->pd->pdef_sbdamp;
 						deflected = 2;
 					}
-					if (mface->v4) { /* quad */
-						/* switch origin to be nv4 */
-						sub_v3_v3v3(edge1, nv3, nv4);
-						sub_v3_v3v3(edge2, nv1, nv4);
-
-						cross_v3_v3v3(d_nvect, edge2, edge1);
-						normalize_v3(d_nvect);
-						if (isect_line_tri_v3( edge_v1, edge_v2, nv1, nv3, nv4, &t, NULL)) {
-							float v1[3], v2[3];
-							float intrusiondepth, i1, i2;
-							sub_v3_v3v3(v1, edge_v1, nv4);
-							sub_v3_v3v3(v2, edge_v2, nv4);
-							i1 = dot_v3v3(v1, d_nvect);
-							i2 = dot_v3v3(v2, d_nvect);
-							intrusiondepth = -min_ff(i1, i2) / el;
-
-
-							Vec3PlusStVec(force, intrusiondepth, d_nvect);
-							*damp=ob->pd->pdef_sbdamp;
-							deflected = 2;
-						}
-					}
-					mface++;
+
 					mima++;
+					vt++;
 				}/* while a */
 			} /* if (ob->pd && ob->pd->deflect) */
 			BLI_ghashIterator_step(ihash);
@@ -1629,10 +1509,10 @@ static void _scan_for_ext_spring_forces(Scene *scene, Object *ob, float timenow,
 					normalize_v3(vel);
 					if (ob->softflag & OB_SB_AERO_ANGLE) {
 						normalize_v3(sp);
-						Vec3PlusStVec(bs->ext_force, f * (1.0f - fabsf(dot_v3v3(vel, sp))), vel);
+						madd_v3_v3fl(bs->ext_force, vel, f * (1.0f - fabsf(dot_v3v3(vel, sp))));
 					}
 					else {
-						Vec3PlusStVec(bs->ext_force, f, vel); // to keep compatible with 2.45 release files
+						madd_v3_v3fl(bs->ext_force, vel, f); // to keep compatible with 2.45 release files
 					}
 				}
 				/* --- springs seeing wind */
@@ -1745,15 +1625,16 @@ static int choose_winner(float*w, float* pos, float*a, float*b, float*c, float*c
 
 
 
-static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3], float *damp,
-									 float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner,
-									 float time, float vel[3], float *intrusion)
+static int sb_detect_vertex_collisionCached(
+        float opco[3], float facenormal[3], float *damp,
+        float force[3], unsigned int UNUSED(par_layer), struct Object *vertexowner,
+        float time, float vel[3], float *intrusion)
 {
 	Object *ob= NULL;
 	GHash *hash;
 	GHashIterator *ihash;
-	float nv1[3], nv2[3], nv3[3], nv4[3], edge1[3], edge2[3], d_nvect[3], dv1[3], ve[3], avel[3] = {0.0, 0.0, 0.0},
-	      vv1[3], vv2[3], vv3[3], vv4[3], coledge[3] = {0.0f, 0.0f, 0.0f}, mindistedge = 1000.0f,
+	float nv1[3], nv2[3], nv3[3], edge1[3], edge2[3], d_nvect[3], dv1[3], ve[3], avel[3] = {0.0, 0.0, 0.0},
+	      vv1[3], vv2[3], vv3[3], coledge[3] = {0.0f, 0.0f, 0.0f}, mindistedge = 1000.0f,
 	      outerforceaccu[3], innerforceaccu[3],
 	      facedist, /* n_mag, */ /* UNUSED */ force_mag_norm, minx, miny, minz, maxx, maxy, maxz,
 	      innerfacethickness = -0.5f, outerfacethickness = 0.2f,
@@ -1773,17 +1654,17 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 		{
 			/* only with deflecting set */
 			if (ob->pd && ob->pd->deflect) {
-				MFace *mface= NULL;
-				MVert *mvert= NULL;
-				MVert *mprevvert= NULL;
-				ccdf_minmax *mima= NULL;
+				const MVert *mvert = NULL;
+				const MVert *mprevvert = NULL;
+				const MVertTri *vt = NULL;
+				const ccdf_minmax *mima = NULL;
 
 				if (ccdm) {
-					mface= ccdm->mface;
-					mvert= ccdm->mvert;
-					mprevvert= ccdm->mprevvert;
-					mima= ccdm->mima;
-					a = ccdm->totface;
+					mvert = ccdm->mvert;
+					mprevvert = ccdm->mprevvert;
+					vt = ccdm->tri;
+					mima = ccdm->mima;
+					a = ccdm->tri_num;
 
 					minx = ccdm->bbmin[0];
 					miny = ccdm->bbmin[1];
@@ -1822,27 +1703,23 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 				avel[0]=avel[1]=avel[2]=0.0f;
 				/* use mesh*/
 				while (a--) {
-					if (
-						(opco[0] < mima->minx) ||
-						(opco[0] > mima->maxx) ||
-						(opco[1] < mima->miny) ||
-						(opco[1] > mima->maxy) ||
-						(opco[2] < mima->minz) ||
-						(opco[2] > mima->maxz)
-						) {
-							mface++;
-							mima++;
-							continue;
+					if ((opco[0] < mima->minx) ||
+					    (opco[0] > mima->maxx) ||
+					    (opco[1] < mima->miny) ||
+					    (opco[1] > mima->maxy) ||
+					    (opco[2] < mima->minz) ||
+					    (opco[2] > mima->maxz))
+					{
+						mima++;
+						vt++;
+						continue;
 					}
 
 					if (mvert) {
 
-						copy_v3_v3(nv1, mvert[mface->v1].co);
-						copy_v3_v3(nv2, mvert[mface->v2].co);
-						copy_v3_v3(nv3, mvert[mface->v3].co);
-						if (mface->v4) {
-							copy_v3_v3(nv4, mvert[mface->v4].co);
-						}
+						copy_v3_v3(nv1, mvert[vt->tri[0]].co);
+						copy_v3_v3(nv2, mvert[vt->tri[1]].co);
+						copy_v3_v3(nv3, mvert[vt->tri[2]].co);
 
 						if (mprevvert) {
 							/* grab the average speed of the collider vertices
@@ -1851,26 +1728,18 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 							since the AABB reduced propabitlty to get here drasticallly
 							it might be a nice tradeof CPU <--> memory
 							*/
-							sub_v3_v3v3(vv1, nv1, mprevvert[mface->v1].co);
-							sub_v3_v3v3(vv2, nv2, mprevvert[mface->v2].co);
-							sub_v3_v3v3(vv3, nv3, mprevvert[mface->v3].co);
-							if (mface->v4) {
-								sub_v3_v3v3(vv4, nv4, mprevvert[mface->v4].co);
-							}
+							sub_v3_v3v3(vv1, nv1, mprevvert[vt->tri[0]].co);
+							sub_v3_v3v3(vv2, nv2, mprevvert[vt->tri[1]].co);
+							sub_v3_v3v3(vv3, nv3, mprevvert[vt->tri[2]].co);
 
 							mul_v3_fl(nv1, time);
-							Vec3PlusStVec(nv1, (1.0f-time), mprevvert[mface->v1].co);
+							madd_v3_v3fl(nv1, mprevvert[vt->tri[0]].co, 1.0f - time);
 
 							mul_v3_fl(nv2, time);
-							Vec3PlusStVec(nv2, (1.0f-time), mprevvert[mface->v2].co);
+							madd_v3_v3fl(nv2, mprevvert[vt->tri[1]].co, 1.0f - time);
 
 							mul_v3_fl(nv3, time);
-							Vec3PlusStVec(nv3, (1.0f-time), mprevvert[mface->v3].co);
-
-							if (mface->v4) {
-								mul_v3_fl(nv4, time);
-								Vec3PlusStVec(nv4, (1.0f-time), mprevvert[mface->v4].co);
-							}
+							madd_v3_v3fl(nv3, mprevvert[vt->tri[2]].co, 1.0f - time);
 						}
 					}
 
@@ -1893,12 +1762,12 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 							*damp=ob->pd->pdef_sbdamp;
 							if (facedist > 0.0f) {
 								*damp *= (1.0f - facedist/outerfacethickness);
-								Vec3PlusStVec(outerforceaccu, force_mag_norm, d_nvect);
+								madd_v3_v3fl(outerforceaccu, d_nvect, force_mag_norm);
 								deflected = 3;
 
 							}
 							else {
-								Vec3PlusStVec(innerforceaccu, force_mag_norm, d_nvect);
+								madd_v3_v3fl(innerforceaccu, d_nvect, force_mag_norm);
 								if (deflected < 2) deflected = 2;
 							}
 							if ((mprevvert) && (*damp > 0.0f)) {
@@ -1910,99 +1779,9 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 							ci++;
 						}
 					}
-					if (mface->v4) { /* quad */
-						/* switch origin to be nv4 */
-						sub_v3_v3v3(edge1, nv3, nv4);
-						sub_v3_v3v3(edge2, nv1, nv4);
-						sub_v3_v3v3(dv1, opco, nv4); /* abuse dv1 to have vertex in question at *origin* of triangle */
-
-						cross_v3_v3v3(d_nvect, edge2, edge1);
-						/* n_mag = */ /* UNUSED */ normalize_v3(d_nvect);
-						facedist = dot_v3v3(dv1, d_nvect);
-
-						if ((facedist > innerfacethickness) && (facedist < outerfacethickness)) {
-							if (isect_point_tri_prism_v3(opco, nv1, nv3, nv4) ) {
-								force_mag_norm =(float)exp(-ee*facedist);
-								if (facedist > outerfacethickness*ff)
-									force_mag_norm =(float)force_mag_norm*fa*(facedist - outerfacethickness)*(facedist - outerfacethickness);
-								*damp=ob->pd->pdef_sbdamp;
-								if (facedist > 0.0f) {
-									*damp *= (1.0f - facedist/outerfacethickness);
-									Vec3PlusStVec(outerforceaccu, force_mag_norm, d_nvect);
-									deflected = 3;
-
-								}
-								else {
-									Vec3PlusStVec(innerforceaccu, force_mag_norm, d_nvect);
-									if (deflected < 2) deflected = 2;
-								}
-
-								if ((mprevvert) && (*damp > 0.0f)) {
-									choose_winner(ve, opco, nv1, nv3, nv4, vv1, vv3, vv4);
-									add_v3_v3(avel, ve);
-									cavel ++;
-								}
-								*intrusion += facedist;
-								ci++;
-							}
-
-						}
-						if ((deflected < 2)&& (G.debug_value != 444)) { /* we did not hit a face until now */
-							/* see if 'outer' hits an edge */
-							float dist;
-
-							closest_to_line_segment_v3(ve, opco, nv1, nv2);
-							sub_v3_v3v3(ve, opco, ve);
-							dist = normalize_v3(ve);
-							if ((dist < outerfacethickness)&&(dist < mindistedge )) {
-								copy_v3_v3(coledge, ve);
-								mindistedge = dist,
-								deflected=1;
-							}
 
-							closest_to_line_segment_v3(ve, opco, nv2, nv3);
-							sub_v3_v3v3(ve, opco, ve);
-							dist = normalize_v3(ve);
-							if ((dist < outerfacethickness)&&(dist < mindistedge )) {
-								copy_v3_v3(coledge, ve);
-								mindistedge = dist,
-								deflected=1;
-							}
-
-							closest_to_line_segment_v3(ve, opco, nv3, nv1);
-							sub_v3_v3v3(ve, opco, ve);
-							dist = normalize_v3(ve);
-							if ((dist < outerfacethickness)&&(dist < mindistedge )) {
-								copy_v3_v3(coledge, ve);
-								mindistedge = dist,
-								deflected=1;
-							}
-							if (mface->v4) { /* quad */
-								closest_to_line_segment_v3(ve, opco, nv3, nv4);
-								sub_v3_v3v3(ve, opco, ve);
-								dist = normalize_v3(ve);
-								if ((dist < outerfacethickness)&&(dist < mindistedge )) {
-									copy_v3_v3(coledge, ve);
-									mindistedge = dist,
-										deflected=1;
-								}
-
-								closest_to_line_segment_v3(ve, opco, nv1, nv4);
-								sub_v3_v3v3(ve, opco, ve);
-								dist = normalize_v3(ve);
-								if ((dist < outerfacethickness)&&(dist < mindistedge )) {
-									copy_v3_v3(coledge, ve);
-									mindistedge = dist,
-										deflected=1;
-								}
-
-							}
-
-
-						}
-					}
-					mface++;
 					mima++;
+					vt++;
 				}/* while a */
 			} /* if (ob->pd && ob->pd->deflect) */
 			BLI_ghashIterator_step(ihash);
@@ -2013,7 +1792,7 @@ static int sb_detect_vertex_collisionCached(float opco[3], float facenormal[3],
 		force_mag_norm =(float)exp(-ee*mindistedge);
 		if (mindistedge > outerfacethickness*ff)
 			force_mag_norm =(float)force_mag_norm*fa*(mindistedge - outerfacethickness)*(mindistedge - outerfacethickness);
-		Vec3PlusStVec(force, force_mag_norm, coledge);
+		madd_v3_v3fl(force, coledge, force_mag_norm);
 		*damp=ob->pd->pdef_sbdamp;
 		if (mindistedge > 0.0f) {
 			*damp *= (1.0f - mindistedge/outerfacethickness);
@@ -2149,7 +1928,7 @@ static void sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, floa
 	}
 
 
-	Vec3PlusStVec(bp1->force, (bs->len - distance)*forcefactor, dir);
+	madd_v3_v3fl(bp1->force, dir, (bs->len - distance) * forcefactor);
 
 	/* do bp1 <--> bp2 viscous */
 	sub_v3_v3v3(dvel, bp1->vec, bp2->vec);
@@ -2157,7 +1936,7 @@ static void sb_spring_force(Object *ob, int bpi, BodySpring *bs, float iks, floa
 	absvel  = normalize_v3(dvel);
 	projvel = dot_v3v3(dir, dvel);
 	kd     *= absvel * projvel;
-	Vec3PlusStVec(bp1->force, -kd, dir);
+	madd_v3_v3fl(bp1->force, dir, -kd);
 
 	/* do jacobian stuff if needed */
 	if (nl_flags & NLF_BUILD) {
@@ -2250,15 +2029,15 @@ static int _softbody_calc_forces_slice_in_a_thread(Scene *scene, Object *ob, flo
 						sub_v3_v3v3(dvel, velcenter, bp->vec);
 						mul_v3_fl(dvel, _final_mass(ob, bp));
 
-						Vec3PlusStVec(bp->force, f*(1.0f-sb->balldamp), def);
-						Vec3PlusStVec(bp->force, sb->balldamp, dvel);
+						madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
+						madd_v3_v3fl(bp->force, dvel, sb->balldamp);
 
 						/* exploit force(a, b) == -force(b, a) part2/2 */
 						sub_v3_v3v3(dvel, velcenter, obp->vec);
 						mul_v3_fl(dvel, _final_mass(ob, bp));
 
-						Vec3PlusStVec(obp->force, sb->balldamp, dvel);
-						Vec3PlusStVec(obp->force, -f*(1.0f-sb->balldamp), def);
+						madd_v3_v3fl(obp->force, dvel, sb->balldamp);
+						madd_v3_v3fl(obp->force, def, -f * (1.0f - sb->balldamp));
 					}
 				}
 			}
@@ -2352,9 +2131,9 @@ static int _softbody_calc_forces_slice_in_a_thread(Scene *scene, Object *ob, flo
 					}
 
 					sub_v3_v3v3(cfforce, bp->vec, vel);
-					Vec3PlusStVec(bp->force, -cf*50.0f, cfforce);
+					madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
 
-					Vec3PlusStVec(bp->force, kd, defforce);
+					madd_v3_v3fl(bp->force, defforce, kd);
 				}
 
 			}
@@ -2619,8 +2398,8 @@ static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, floa
 							sub_v3_v3v3(dvel, velcenter, bp->vec);
 							mul_v3_fl(dvel, _final_mass(ob, bp));
 
-							Vec3PlusStVec(bp->force, f*(1.0f-sb->balldamp), def);
-							Vec3PlusStVec(bp->force, sb->balldamp, dvel);
+							madd_v3_v3fl(bp->force, def, f * (1.0f - sb->balldamp));
+							madd_v3_v3fl(bp->force, dvel, sb->balldamp);
 
 							if (nl_flags & NLF_BUILD) {
 								//int ia =3*(sb->totpoint-a);
@@ -2650,10 +2429,8 @@ static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, floa
 							sub_v3_v3v3(dvel, velcenter, obp->vec);
 							mul_v3_fl(dvel, (_final_mass(ob, bp)+_final_mass(ob, obp))/2.0f);
 
-							Vec3PlusStVec(obp->force, sb->balldamp, dvel);
-							Vec3PlusStVec(obp->force, -f*(1.0f-sb->balldamp), def);
-
-
+							madd_v3_v3fl(obp->force, dvel, sb->balldamp);
+							madd_v3_v3fl(obp->force, def, -f * (1.0f - sb->balldamp));
 						}
 					}
 				}
@@ -2771,12 +2548,12 @@ static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, floa
 								in heun step No1 and leave the heun step No2 adapt to it
 								so we kind of introduced a implicit solver for this case
 								*/
-								Vec3PlusStVec(bp->pos, -intrusion, facenormal);
+								madd_v3_v3fl(bp->pos, facenormal, -intrusion);
 							}
 							else {
 
 								sub_v3_v3v3(cfforce, bp->vec, vel);
-								Vec3PlusStVec(bp->force, -cf*50.0f, cfforce);
+								madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
 							}
 
 
@@ -2786,9 +2563,9 @@ static void softbody_calc_forces(Scene *scene, Object *ob, float forcetime, floa
 						}
 						else {
 							sub_v3_v3v3(cfforce, bp->vec, vel);
-							Vec3PlusStVec(bp->force, -cf*50.0f, cfforce);
+							madd_v3_v3fl(bp->force, cfforce, -cf * 50.0f);
 						}
-						Vec3PlusStVec(bp->force, kd, defforce);
+						madd_v3_v3fl(bp->force, defforce, kd);
 						if (nl_flags & NLF_BUILD) {
 							// int ia =3*(sb->totpoint-a);
 							// int op =3*sb->totpoint;