Added sphere cast (tought it doenst seems to solve the intersection problem)

Fixed SweepingSphereIntersectsTriangleUV

2 files changed, 230 insertions, 248 deletions

diff --git a/source/blender/blenkernel/intern/shrinkwrap.c b/source/blender/blenkernel/intern/shrinkwrap.c
index ee3bf993edb..4c01e5fb84a 100644
--- a/source/blender/blenkernel/intern/shrinkwrap.c
+++ b/source/blender/blenkernel/intern/shrinkwrap.c
@@ -92,100 +92,35 @@ static float ray_intersect_plane(const float *point, const float *dir, const flo
 
 
 /* ray - triangle */
-
 #define ISECT_EPSILON 1e-6
-float ray_tri_intersection(const BVHTreeRay *ray, const float m_dist, const float *v0, const float *v1, const float *v2)
+static float ray_tri_intersection(const BVHTreeRay *ray, const float m_dist, const float *v0, const float *v1, const float *v2)
 {
 	float dist;
+
 	if(RayIntersectsTriangle(ray->origin, ray->direction, v0, v1, v2, &dist, NULL))
 		return dist;
 
-/*	
-	float pnormal[3];
-	float dist;
-
-	CalcNormFloat(v0, v1, v2, pnormal);
-	dist = ray_intersect_plane(ray->origin, ray->direction, v0, pnormal);
-
-	if(dist > 0 && dist < m_dist)
-	{
-		float tmp[3], nearest[3];
-		VECADDFAC(tmp, ray->origin, ray->direction, dist);
-
-		if(fabs(nearest_point_in_tri_surface(tmp, v0, v1, v2, nearest)) < 0.0001)
-			return dist;
-	}
-*/
-
-/*
-	float x0,x1,x2,t00,t01,t02,t10,t11,t12,r0,r1,r2;
-	float m0, m1, m2, divdet, det1;
-	float u,v;
-	float cros0, cros1, cros2;
-	float labda;
-
-	float t0[3], t1[3];
-
-	VECSUB(t0, v2, v0);
-	VECSUB(t1, v2, v1);
-
-	Crossf(x, t1, ray->direction);
-
-	divdet = INPR( t0, x );
-
-	VECSUB( m, ray->origin, v2 );
-	det1 = INPR(m, x);
-
-	Crossf(cros, m, t0);
-
-	if(divdet == 0.0)
-		return FLT_MAX;
-
-	
-
-/ *
-	t00= co3[0]-co1[0];
-	t01= co3[1]-co1[1];
-	t02= co3[2]-co1[2];
-	t10= co3[0]-co2[0];
-	t11= co3[1]-co2[1];
-	t12= co3[2]-co2[2];
-	
-	r0= ray->direction[0];
-	r1= ray->direction[1];
-	r2= ray->direction[2];
-	
-	x0= t12*r1-t11*r2;
-	x1= t10*r2-t12*r0;
-	x2= t11*r0-t10*r1;
-
-	divdet= t00*x0+t01*x1+t02*x2;
-
-	m0= ray->origin[0]-co3[0];
-	m1= ray->origin[0]-co3[1];
-	m2= ray->origin[0]-co3[2];
-	det1= m0*x0+m1*x1+m2*x2;
-
-	cros0= m1*t02-m2*t01;
-	cros1= m2*t00-m0*t02;
-	cros2= m0*t01-m1*t00;
+	return FLT_MAX;
+}
 
+static float sphereray_tri_intersection(const BVHTreeRay *ray, float radius, const float m_dist, const float *v0, const float *v1, const float *v2)
+{
 	
-	if(divdet==0.0f)
-		return FLT_MAX;
+	float dist, idist;
+	float p0[3], p1[3];
+	float plane_normal[3], hit_point[3];
 
-	divdet= 1.0f/divdet;
-	u= det1*divdet;
-	v= divdet*(cros0*r0 + cros1*r1 + cros2*r2);
+	CalcNormFloat((float*)v0, (float*)v1, (float*)v2, plane_normal);
 
-	labda= divdet*(cros0*t10 + cros1*t11 + cros2*t12);
+	dist = ray_intersect_plane(ray->origin, ray->direction, v0, plane_normal);
 
-	if(u<ISECT_EPSILON && u>-(1.0f+ISECT_EPSILON)
-	&& v<ISECT_EPSILON && (u + v) > -(1.0f+ISECT_EPSILON))
+	VECADDFAC( p0, ray->origin, ray->direction, /*dist-radius*/ 0);
+	VECADDFAC( p1, ray->origin, ray->direction, m_dist);
+	if(SweepingSphereIntersectsTriangleUV(p0, p1, radius, v0, v1, v2, &idist, &hit_point))
 	{
-		return labda;
+		return idist * m_dist;
 	}
-*/
+
 	return FLT_MAX;
 }
 
@@ -193,7 +128,28 @@ float ray_tri_intersection(const BVHTreeRay *ray, const float m_dist, const floa
 /*
  * BVH tree from mesh vertices
  */
-static BVHTree* bvhtree_from_mesh_verts(DerivedMesh *mesh)
+typedef struct BVHMeshCallbackUserdata
+{
+	//Mesh represented on this BVH
+	DerivedMesh *mesh;
+	MVert *vert;
+	MFace *face;
+
+	//radius for sphere cast
+	float sphere_radius;
+
+} BVHMeshCallbackUserdata;
+
+
+static void bvhtree_meshcallbackdata_init(BVHMeshCallbackUserdata *data, DerivedMesh *mesh, float cast_radius)
+{
+	data->mesh = mesh;
+	data->vert = mesh->getVertDataArray(mesh, CD_MVERT);
+	data->face = mesh->getFaceDataArray(mesh, CD_MFACE);
+	data->sphere_radius = cast_radius;
+}
+
+static BVHTree* bvhtree_from_mesh_verts(DerivedMesh *mesh, float epsilon)
 {
 	int i;
 	int numVerts= mesh->getNumVerts(mesh);
@@ -211,7 +167,7 @@ static BVHTree* bvhtree_from_mesh_verts(DerivedMesh *mesh)
 	return tree;
 }
 
-static BVHTree* bvhtree_from_mesh_tri(DerivedMesh *mesh)
+static BVHTree* bvhtree_from_mesh_tri(DerivedMesh *mesh, float epsilon)
 {
 	int i;
 	int numFaces= mesh->getNumFaces(mesh), totFaces;
@@ -224,7 +180,7 @@ static BVHTree* bvhtree_from_mesh_tri(DerivedMesh *mesh)
 		if(face[i].v4) totFaces++;
 
 	/* Create a bvh-tree of the given target */
-	tree = BLI_bvhtree_new(totFaces, 0, 2, 6);
+	tree = BLI_bvhtree_new(totFaces, epsilon, 2, 6);
 	if(tree != NULL)
 	{
 		for(i = 0; i < numFaces; i++)
@@ -250,11 +206,12 @@ static BVHTree* bvhtree_from_mesh_tri(DerivedMesh *mesh)
 	return tree;
 }
 
+
 static float mesh_tri_nearest_point(void *userdata, int index, const float *co, float *nearest)
 {
-	DerivedMesh *mesh = (DerivedMesh*)(userdata);
-	MVert *vert	= (MVert*)mesh->getVertDataArray(mesh, CD_MVERT);
-	MFace *face = (MFace*)mesh->getFaceDataArray(mesh, CD_MFACE) + index/2;
+	const BVHMeshCallbackUserdata *data = (BVHMeshCallbackUserdata*) userdata;
+	MVert *vert	= data->vert;
+	MFace *face = data->face + index / 2;
 
 	if(index & 1)
 		return nearest_point_in_tri_surface(co, vert[ face->v1 ].co, vert[ face->v3 ].co, vert[ face->v4 ].co, nearest);
@@ -262,11 +219,13 @@ static float mesh_tri_nearest_point(void *userdata, int index, const float *co,
 		return nearest_point_in_tri_surface(co, vert[ face->v1 ].co, vert[ face->v2 ].co, vert[ face->v3 ].co, nearest);
 }
 
+
+
 static float mesh_tri_spherecast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
 {
-	DerivedMesh *mesh = (DerivedMesh*)(userdata);
-	MVert *vert	= (MVert*)mesh->getVertDataArray(mesh, CD_MVERT);
-	MFace *face = (MFace*)mesh->getFaceDataArray(mesh, CD_MFACE) + index/2;
+	const BVHMeshCallbackUserdata *data = (BVHMeshCallbackUserdata*) userdata;
+	MVert *vert	= data->vert;
+	MFace *face = data->face + index / 2;
 
 	const float *t0, *t1, *t2;
 	float dist;
@@ -276,29 +235,38 @@ static float mesh_tri_spherecast(void *userdata, int index, const BVHTreeRay *ra
 	else
 		t0 = &vert[ face->v1 ].co, t1 = &vert[ face->v2 ].co, t2 = &vert[ face->v3 ].co;
 
-
-	dist = ray_tri_intersection(ray, hit->dist, t0, t1, t2);
-	if(dist < hit->dist)
+	dist = sphereray_tri_intersection(ray, data->sphere_radius, hit->dist, t0, t1, t2);
+	if(dist >= 0 && dist < hit->dist)
 	{
 		hit->index = index;
-		hit->dist = fabs(dist);
-		VECADDFAC(hit->co, ray->origin, ray->direction, hit->dist);
+		hit->dist = dist;
+		VECADDFAC(hit->co, ray->origin, ray->direction, dist);
 	}
+	return dist;
+}
 
+static float mesh_tri_raycast(void *userdata, int index, const BVHTreeRay *ray, BVHTreeRayHit *hit)
+{
+	const BVHMeshCallbackUserdata *data = (BVHMeshCallbackUserdata*) userdata;
+	MVert *vert	= data->vert;
+	MFace *face = data->face + index/2;
 
+	const float *t0, *t1, *t2;
+	float dist;
 
-/*
-	VECADDFAC(v0, ray->origin, ray->direction, 0);
-	VECADDFAC(v1, ray->origin, ray->direction, hit->dist);
+	if(index & 1)
+		t0 = &vert[ face->v1 ].co, t1 = &vert[ face->v3 ].co, t2 = &vert[ face->v4 ].co;
+	else
+		t0 = &vert[ face->v1 ].co, t1 = &vert[ face->v2 ].co, t2 = &vert[ face->v3 ].co;
 
-	if(SweepingSphereIntersectsTriangleUV(v0, v1, 0.1f, t0, t1, t2, &lambda, hit_point))
+	dist = ray_tri_intersection(ray, hit->dist, t0, t1, t2);
+	if(dist >= 0 && dist < hit->dist)
 	{
 		hit->index = index;
-		hit->dist *= lambda;
-		VECADDFAC(hit->co, ray->origin, ray->direction, hit->dist);
-	}	
-*/
-
+		hit->dist = dist;
+		VECADDFAC(hit->co, ray->origin, ray->direction, dist);
+	}
+	return dist;
 }
 /*
  * Raytree from mesh
@@ -916,6 +884,7 @@ static void shrinkwrap_removeUnused(ShrinkwrapCalcData *calc)
 		//Ignore a face were not a single vertice moved
 		if(res == 0) continue;
 
+
 		//Only 1 vertice moved.. (if its a quad.. remove the vertice oposite to it)
 		if(res == 1 && face[i].v4)
 		{
@@ -942,6 +911,27 @@ static void shrinkwrap_removeUnused(ShrinkwrapCalcData *calc)
 
 			face[i].v4 = 0;	//this quad turned on a tri
 		}
+		
+/*
+		if(face[i].v4 && res == 3)
+		{
+			if(!bitset_get(moved_verts, face[i].v1))
+			{
+				face[i].v1 = face[i].v4;
+			}
+			else if(!bitset_get(moved_verts, face[i].v2))
+			{
+				face[i].v2 = face[i].v3;
+				face[i].v3 = face[i].v4;
+			}
+			else if(!bitset_get(moved_verts, face[i].v3))
+			{
+				face[i].v3 = face[i].v4;
+			}
+
+			face[i].v4 = 0;	//this quad turned on a tri
+		}
+*/
 
 		bitset_set(used_faces, i);	//Mark face to maintain
 		numUsedFaces++;
@@ -1140,12 +1130,13 @@ DerivedMesh *shrinkwrapModifier_do(ShrinkwrapModifierData *smd, Object *ob, Deri
 			case MOD_SHRINKWRAP_NORMAL:
 
 				if(calc.smd->shrinkOpts & MOD_SHRINKWRAP_REMOVE_UNPROJECTED_FACES)
-					calc.moved = bitset_new( calc.final->getNumVerts(calc.final), "shrinkwrap bitset data");
+				calc.moved = bitset_new( calc.final->getNumVerts(calc.final), "shrinkwrap bitset data");
 
+/*
 				BENCH(shrinkwrap_calc_normal_projection_raytree(&calc));
 				calc.final->release( calc.final );
-
-				calc.final = CDDM_copy(calc.original);
+*/
+//				calc.final = CDDM_copy(calc.original);
 				BENCH(shrinkwrap_calc_normal_projection(&calc));
 //				BENCH(shrinkwrap_calc_foreach_vertex(&calc, bruteforce_shrinkwrap_calc_normal_projection));
 
@@ -1200,7 +1191,7 @@ void shrinkwrap_calc_nearest_vertex(ShrinkwrapCalcData *calc)
 	BENCH_VAR(query);
 
 
-	BENCH(tree = bvhtree_from_mesh_verts(calc->target));
+	BENCH(tree = bvhtree_from_mesh_verts(calc->target, 0.0));
 	if(tree == NULL) return OUT_OF_MEMORY();
 
 	//Setup nearest
@@ -1352,6 +1343,8 @@ void shrinkwrap_calc_normal_projection(ShrinkwrapCalcData *calc)
 	//setup raytracing
 	BVHTree *tree	= NULL;
 	BVHTreeRayHit hit;
+	BVHMeshCallbackUserdata userdata;
+	BVHTree_RayCastCallback callback = NULL;
 
 	int	numVerts;
 	MVert *vert = NULL;
@@ -1360,9 +1353,10 @@ void shrinkwrap_calc_normal_projection(ShrinkwrapCalcData *calc)
 	if( (use_normal & (MOD_SHRINKWRAP_ALLOW_INVERTED_NORMAL | MOD_SHRINKWRAP_ALLOW_DEFAULT_NORMAL)) == 0)
 		return;	//Nothing todo
 
-	BENCH(tree = bvhtree_from_mesh_tri(calc->target));
+	BENCH(tree = bvhtree_from_mesh_tri(calc->target, calc->keptDist));
 	if(tree == NULL) return OUT_OF_MEMORY();
-
+	bvhtree_meshcallbackdata_init(&userdata, calc->target, calc->keptDist);
+	callback = calc->keptDist > 0 ? mesh_tri_spherecast : mesh_tri_raycast;
 
 	//Project each vertex along normal
 	numVerts= calc->final->getNumVerts(calc->final);
@@ -1388,13 +1382,13 @@ void shrinkwrap_calc_normal_projection(ShrinkwrapCalcData *calc)
 
 		if(use_normal & MOD_SHRINKWRAP_ALLOW_DEFAULT_NORMAL)
 		{
-			BLI_bvhtree_ray_cast(tree, tmp_co, tmp_no, &hit, mesh_tri_spherecast, calc->target);
+			BLI_bvhtree_ray_cast(tree, tmp_co, tmp_no, &hit, callback, &userdata);
 		}
 
 		if(use_normal & MOD_SHRINKWRAP_ALLOW_INVERTED_NORMAL)
 		{
 			float inv[3] = { -tmp_no[0], -tmp_no[1], -tmp_no[2] };
-			BLI_bvhtree_ray_cast(tree, tmp_co, inv, &hit, mesh_tri_spherecast, calc->target);
+			BLI_bvhtree_ray_cast(tree, tmp_co, inv, &hit, callback, &userdata);
 		}
 
 		if(hit.index != -1)
@@ -1424,6 +1418,7 @@ void shrinkwrap_calc_nearest_surface_point(ShrinkwrapCalcData *calc)
 
 	BVHTree *tree	= NULL;
 	BVHTreeNearest nearest;
+	BVHMeshCallbackUserdata userdata;
 
 	int	numVerts;
 	MVert *vert = NULL;
@@ -1431,8 +1426,9 @@ void shrinkwrap_calc_nearest_surface_point(ShrinkwrapCalcData *calc)
 
 
 	//Create a bvh-tree of the given target
-	tree = bvhtree_from_mesh_tri(calc->target);
+	tree = bvhtree_from_mesh_tri(calc->target, 0.0);
 	if(tree == NULL) return OUT_OF_MEMORY();
+	bvhtree_meshcallbackdata_init(&userdata, calc->target, 0.0);
 
 	//Setup nearest
 	nearest.index = -1;
@@ -1458,7 +1454,7 @@ void shrinkwrap_calc_nearest_surface_point(ShrinkwrapCalcData *calc)
 		}
 		else nearest.dist = FLT_MAX;
 
-		index = BLI_bvhtree_find_nearest(tree, tmp_co, &nearest, mesh_tri_nearest_point, calc->target);
+		index = BLI_bvhtree_find_nearest(tree, tmp_co, &nearest, mesh_tri_nearest_point, &userdata);
 
 		if(index != -1)
 		{
diff --git a/source/blender/blenlib/intern/arithb.c b/source/blender/blenlib/intern/arithb.c
index 2084ab3da5f..a366862d064 100644
--- a/source/blender/blenlib/intern/arithb.c
+++ b/source/blender/blenlib/intern/arithb.c
@@ -55,11 +55,10 @@
 #include <stdio.h>
 #include "BLI_arithb.h"
 #include "BLI_memarena.h"
+#include "BKE_utildefines.h"
 
 /* A few small defines. Keep'em local! */
 #define SMALL_NUMBER	1.e-8
-#define ABS(x)	((x) < 0 ? -(x) : (x))
-#define SWAP(type, a, b)	{ type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
 
 
 #if defined(WIN32) || defined(__APPLE__)
@@ -3740,12 +3739,50 @@ int RayIntersectsTriangle(float p1[3], float d[3], float v0[3], float v1[3], flo
 
 /* Adapted from the paper by Kasper Fauerby */
 /* "Improved Collision detection and Response" */
+static int getLowestRoot(float a, float b, float c, float maxR, float* root)
+{
+	// Check if a solution exists
+	float determinant = b*b - 4.0f*a*c;
+
+	// If determinant is negative it means no solutions.
+	if (determinant >= 0.0f)
+	{
+		// calculate the two roots: (if determinant == 0 then
+		// x1==x2 but let’s disregard that slight optimization)
+		float sqrtD = sqrt(determinant);
+		float r1 = (-b - sqrtD) / (2.0f*a);
+		float r2 = (-b + sqrtD) / (2.0f*a);
+		
+		// Sort so x1 <= x2
+		if (r1 > r2)
+			SWAP( float, r1, r2);
+
+		// Get lowest root:
+		if (r1 > 0.0f && r1 < maxR)
+		{
+			*root = r1;
+			return 1;
+		}
+
+		// It is possible that we want x2 - this can happen
+		// if x1 < 0
+		if (r2 > 0.0f && r2 < maxR)
+		{
+			*root = r2;
+			return 1;
+		}
+	}
+	// No (valid) solutions
+	return 0;
+}
+
 int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, float v0[3], float v1[3], float v2[3], float *lambda, float *ipoint)
 {
 	float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
-	float a, b, c, d, e, x, y, z, t, t0, t1, radius2=radius*radius;
+	float a, b, c, d, e, x, y, z, radius2=radius*radius;
 	float elen2,edotv,edotbv,nordotv,vel2;
-	int embedded_in_plane=0, found_by_sweep=0;
+	float newLambda;
+	int found_by_sweep=0;
 
 	VecSubf(e1,v1,v0);
 	VecSubf(e2,v2,v0);
@@ -3754,44 +3791,41 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 /*---test plane of tri---*/
 	Crossf(nor,e1,e2);
 	Normalize(nor);
+
 	/* flip normal */
 	if(Inpf(nor,vel)>0.0f) VecMulf(nor,-1.0f);
 	
 	a=Inpf(p1,nor)-Inpf(v0,nor);
-
 	nordotv=Inpf(nor,vel);
 
-	if ((nordotv > -0.000001) && (nordotv < 0.000001)) {
-		if(fabs(a)>=1.0f)
+	if (fabs(nordotv) < 0.000001)
+	{
+		if(fabs(a)>=radius)
+		{
 			return 0;
-		else{
-			embedded_in_plane=1;
-			t0=0.0f;
-			t1=1.0f;
 		}
 	}
-	else{
-		t0=(radius-a)/nordotv;
-		t1=(-radius-a)/nordotv;
-		/* make t0<t1 */
-		if(t0>t1){b=t1; t1=t0; t0=b;}
+	else
+	{
+		float t0=(-a+radius)/nordotv;
+		float t1=(-a-radius)/nordotv;
+
+		if(t0>t1)
+			SWAP(float, t0, t1);
 
 		if(t0>1.0f || t1<0.0f) return 0;
 
 		/* clamp to [0,1] */
-		t0=(t0<0.0f)?0.0f:((t0>1.0f)?1.0:t0);
-		t1=(t1<0.0f)?0.0f:((t1>1.0f)?1.0:t1);
-	}
+		CLAMP(t0, 0.0f, 1.0f);
+		CLAMP(t1, 0.0f, 1.0f);
 
-/*---test inside of tri---*/
-	if(embedded_in_plane==0){
+		/*---test inside of tri---*/
 		/* plane intersection point */
-		VecCopyf(point,vel);
-		VecMulf(point,t0);
-		VecAddf(point,point,p1);
-		VecCopyf(temp,nor);
-		VecMulf(temp,radius);
-		VecSubf(point,point,temp);
+
+		point[0] = p1[0] + vel[0]*t0 - nor[0]*radius;
+		point[1] = p1[1] + vel[1]*t0 - nor[1]*radius;
+		point[2] = p1[2] + vel[2]*t0 - nor[2]*radius;
+
 
 		/* is the point in the tri? */
 		a=Inpf(e1,e1);
@@ -3806,14 +3840,19 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 		y=e*a-d*b;
 		z=x+y-(a*c-b*b);
 
-		if(( ((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y)) ) & 0x80000000){
+
+		if( z <= 0.0f && (x >= 0.0f && y >= 0.0f))
+		{
+		//( ((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y)) ) & 0x80000000){
 			*lambda=t0;
 			VecCopyf(ipoint,point);
 			return 1;
 		}
 	}
 
+
 	*lambda=1.0f;
+
 /*---test points---*/
 	a=vel2=Inpf(vel,vel);
 
@@ -3821,73 +3860,42 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 	VecSubf(temp,p1,v0);
 	b=2.0f*Inpf(vel,temp);
 	c=Inpf(temp,temp)-radius2;
-	d=b*b-4*a*c;
-
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		if(t>0.0 && t < *lambda){
-			*lambda=t;
-			VecCopyf(ipoint,v0);
-			found_by_sweep=1;
-		}
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		VecCopyf(ipoint,v0);
+		found_by_sweep=1;
 	}
 
 	/*v1*/
 	VecSubf(temp,p1,v1);
 	b=2.0f*Inpf(vel,temp);
 	c=Inpf(temp,temp)-radius2;
-	d=b*b-4*a*c;
-
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		if(t>0.0 && t < *lambda){
-			*lambda=t;
-			VecCopyf(ipoint,v1);
-			found_by_sweep=1;
-		}
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		VecCopyf(ipoint,v1);
+		found_by_sweep=1;
 	}
+	
 	/*v2*/
 	VecSubf(temp,p1,v2);
 	b=2.0f*Inpf(vel,temp);
 	c=Inpf(temp,temp)-radius2;
-	d=b*b-4*a*c;
-
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		if(t>0.0 && t < *lambda){
-			*lambda=t;
-			VecCopyf(ipoint,v2);
-			found_by_sweep=1;
-		}
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		VecCopyf(ipoint,v2);
+		found_by_sweep=1;
 	}
 
 /*---test edges---*/
+	VecSubf(e3,v2,v1); //wasnt yet calculated
+
+
 	/*e1*/
 	VecSubf(bv,v0,p1);
+
 	elen2 = Inpf(e1,e1);
 	edotv = Inpf(e1,vel);
 	edotbv = Inpf(e1,bv);
@@ -3895,27 +3903,18 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 	a=elen2*(-Inpf(vel,vel))+edotv*edotv;
 	b=2.0f*(elen2*Inpf(vel,bv)-edotv*edotbv);
 	c=elen2*(radius2-Inpf(bv,bv))+edotbv*edotbv;
-	d=b*b-4*a*c;
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		e=(edotv*t-edotbv)/elen2;
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
 
-		if((e>=0.0f) && (e<=1.0f)){
-			if(t>0.0 && t < *lambda){
-				*lambda=t;
-				VecCopyf(ipoint,e1);
-				VecMulf(ipoint,e);
-				VecAddf(ipoint,ipoint,v0);
-				found_by_sweep=1;
-			}
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			VecCopyf(ipoint,e1);
+			VecMulf(ipoint,e);
+			VecAddf(ipoint,ipoint,v0);
+			found_by_sweep=1;
 		}
 	}
 
@@ -3928,32 +3927,27 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 	a=elen2*(-Inpf(vel,vel))+edotv*edotv;
 	b=2.0f*(elen2*Inpf(vel,bv)-edotv*edotbv);
 	c=elen2*(radius2-Inpf(bv,bv))+edotbv*edotbv;
-	d=b*b-4*a*c;
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		e=(edotv*t-edotbv)/elen2;
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
 
-		if((e>=0.0f) && (e<=1.0f)){
-			if(t>0.0 && t < *lambda){
-				*lambda=t;
-				VecCopyf(ipoint,e2);
-				VecMulf(ipoint,e);
-				VecAddf(ipoint,ipoint,v0);
-				found_by_sweep=1;
-			}
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			VecCopyf(ipoint,e2);
+			VecMulf(ipoint,e);
+			VecAddf(ipoint,ipoint,v0);
+			found_by_sweep=1;
 		}
 	}
 
 	/*e3*/
-	VecSubf(e3,v2,v1);
+	VecSubf(bv,v0,p1);
+	elen2 = Inpf(e1,e1);
+	edotv = Inpf(e1,vel);
+	edotbv = Inpf(e1,bv);
+
 	VecSubf(bv,v1,p1);
 	elen2 = Inpf(e3,e3);
 	edotv = Inpf(e3,vel);
@@ -3962,30 +3956,22 @@ int SweepingSphereIntersectsTriangleUV(float p1[3], float p2[3], float radius, f
 	a=elen2*(-Inpf(vel,vel))+edotv*edotv;
 	b=2.0f*(elen2*Inpf(vel,bv)-edotv*edotbv);
 	c=elen2*(radius2-Inpf(bv,bv))+edotbv*edotbv;
-	d=b*b-4*a*c;
-	if(d>=0.0f){
-		if(d==0.0f)
-			t=-b/2*a;
-		else{
-			z=sqrt(d);
-			x=(-b-z)*0.5/a;
-			y=(-b+z)*0.5/a;
-			t=x<y?x:y;
-		}
 
-		e=(edotv*t-edotbv)/elen2;
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
 
-		if((e>=0.0f) && (e<=1.0f)){
-			if(t>0.0 && t < *lambda){
-				*lambda=t;
-				VecCopyf(ipoint,e3);
-				VecMulf(ipoint,e);
-				VecAddf(ipoint,ipoint,v1);
-				found_by_sweep=1;
-			}
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			VecCopyf(ipoint,e3);
+			VecMulf(ipoint,e);
+			VecAddf(ipoint,ipoint,v1);
+			found_by_sweep=1;
 		}
 	}
 
+
 	return found_by_sweep;
 }
 int AxialLineIntersectsTriangle(int axis, float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)