From e35e4d4add9f7efed40f038977c02820995a891f Mon Sep 17 00:00:00 2001
From: kakukri <kakukri@30fe4595-0a0c-4342-8851-515496e4dcbd>
Date: Sat, 16 Mar 2013 18:16:27 +0000
Subject: [PATCH] Re #620: add sideways sticking when speed is < 0.1 m/s

git-svn-id: https://svn.code.sf.net/p/speed-dreams/code/trunk@5326 30fe4595-0a0c-4342-8851-515496e4dcbd

Former-commit-id: 6c803b39112b9d784b95f94b137ee9315efaf2e5
Former-commit-id: c987681c74dc38c27f9c76aafcb5449e284133f1
---
 src/modules/simu/simuv4/car.cpp   | 22 +++++++++++++++++++++-
 src/modules/simu/simuv4/wheel.cpp |  9 +++++++--
 2 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/src/modules/simu/simuv4/car.cpp b/src/modules/simu/simuv4/car.cpp
index fad9321e..40f3d12c 100644
--- a/src/modules/simu/simuv4/car.cpp
+++ b/src/modules/simu/simuv4/car.cpp
@@ -177,6 +177,8 @@ SimCarUpdateForces(tCar *car)
 	tdble	SinTheta;
 	tdble	Cosz, Sinz;
 	tdble	v, R, Rv, Rm, Rx, Ry;
+	tdble	carspeed;
+	tdble	desiredF, desiredTq;
 	
 	Cosz = car->Cosz = cos(car->DynGCg.pos.az);
 	Sinz = car->Sinz = sin(car->DynGCg.pos.az);
@@ -255,6 +257,24 @@ SimCarUpdateForces(tCar *car)
 	} else {
 		Rm = (tdble) (SIGN(car->DynGCg.vel.az) * R * car->wheelbase / 2.0);
 	}
+	F.M.z -= Rm;
+	
+	/* simulate sticking when car almost stationary */
+	carspeed = car->DynGC.vel.x * car->DynGC.vel.x 
+	      + car->DynGC.vel.y * car->DynGC.vel.y + car->DynGC.vel.z * car->DynGC.vel.z;
+	if ((car->features & FEAT_SLOWGRIP) && ( carspeed < 0.1 ) ) {
+		w = -w; //make it positive
+		/* desired force to stop sideway slide */
+		desiredF = - m * car->DynGC.vel.y / SimDeltaTime;
+		if ( (fabs(desiredF - F.F.y)) < w ) {F.F.y = desiredF;}
+		else if ( (desiredF - F.F.y) > 0.0 ) {F.F.y += w;}
+		else {F.F.y -= w;}
+		/* desired torque to stop yaw */
+		desiredTq = - car->DynGC.vel.az / ( SimDeltaTime * car->Iinv.z );
+		if ( (fabs(desiredTq - F.M.z)) < 0.5 * w * car->wheelbase) {F.M.z = desiredTq;}
+		else if ( (desiredTq - F.M.z) > 0.0 ) {F.M.z += 0.5 * w * car->wheelbase;}
+		else {F.M.z -= 0.5 * w * car->wheelbase;}
+	}
 	
 	/* compute accelerations */
 	car->DynGC.acc.x = F.F.x * minv;
@@ -267,7 +287,7 @@ SimCarUpdateForces(tCar *car)
 	
 	car->DynGCg.acc.ax = car->DynGC.acc.ax = F.M.x * car->Iinv.x;
 	car->DynGCg.acc.ay = car->DynGC.acc.ay = F.M.y * car->Iinv.y;
-	car->DynGCg.acc.az = car->DynGC.acc.az = (F.M.z - Rm) * car->Iinv.z;
+	car->DynGCg.acc.az = car->DynGC.acc.az = F.M.z * car->Iinv.z;
 }
 
 static void
diff --git a/src/modules/simu/simuv4/wheel.cpp b/src/modules/simu/simuv4/wheel.cpp
index ba6d2d7b..ebe64c6f 100644
--- a/src/modules/simu/simuv4/wheel.cpp
+++ b/src/modules/simu/simuv4/wheel.cpp
@@ -210,8 +210,13 @@ void SimWheelUpdateForce(tCar *car, int index)
 		sx = wrl;
 		sy = 0.0f;
 	} else {
-		sx = (vt - wrl) / fabs(vt);
-		sy = sin(sa);
+		if (car->features & FEAT_SLOWGRIP) {
+			sx = (vt - wrl) / MAX(fabs(vt), 1.0); //avoid divergence
+			sy = sin(sa);
+		} else {
+			sx = (vt - wrl) / fabs(vt);
+			sy = sin(sa);
+		}
 	}
 
 	Ft = 0.0f;