Re #150: add gyroscopic forces and non-constant weight force in z direction to simuv2.1

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

Former-commit-id: 5dce93c188260a60ad2ccf37fcd0fa3288dc139d
Former-commit-id: 74682c28d6bd08a68c99dc241e3319a7fae862fa

src/modules/simu/simuv2.1/car.cpp

@@ -167,7 +167,7 @@ SimCarUpdateForces(tCar *car)
 	SinTheta = (-car->wheel[FRNT_RGT].zRoad - car->wheel[REAR_RGT].zRoad
 		+ car->wheel[FRNT_LFT].zRoad + car->wheel[REAR_LFT].zRoad) / (2.0 * car->wheeltrack);
 	F.F.y = -w * SinTheta;
-	F.F.z = w; /* not 3D */
+	F.F.z = w - (F.F.x*F.F.x + F.F.y*F.F.y)/(2.0*w);/*Taylor-polinom of sqrt(w^2-F.F.x^2-F.F.y^2)*/
 	F.M.x = F.M.y = F.M.z = 0;
 	
 	/* Wheels */
@@ -179,10 +179,13 @@ SimCarUpdateForces(tCar *car)
 		/* moments */
 		F.M.x += car->wheel[i].forces.z * car->wheel[i].staticPos.y +
 			car->wheel[i].forces.y * (car->statGC.z + car->wheel[i].rideHeight);
+		F.M.x += car->wheel[i].torques.x;
 		F.M.y -= car->wheel[i].forces.z * car->wheel[i].staticPos.x +
 			car->wheel[i].forces.x * (car->statGC.z + car->wheel[i].rideHeight);
+		F.M.y += car->wheel[i].torques.y;
 		F.M.z += -car->wheel[i].forces.x * car->wheel[i].staticPos.y +
 			car->wheel[i].forces.y * car->wheel[i].staticPos.x;
+		F.M.z += car->wheel[i].torques.z;
 	}
 	
 	/* Aero Drag */

src/modules/simu/simuv2.1/steer.cpp

@@ -51,10 +51,18 @@ SimSteerUpdate(tCar *car)
 	steer2 = atan2((car->wheelbase * tanSteer) , (car->wheelbase - tanSteer * car->wheeltrack));
 
 	if (steer > 0) {
+		car->wheel[FRNT_RGT].torques.x = 
+		car->wheel->cosax * (steer2 - car->wheel[FRNT_RGT].steer) * car->wheel[FRNT_RGT].prespinVel * car->wheel[FRNT_RGT].I / SimDeltaTime;
 		car->wheel[FRNT_RGT].steer = steer2;
+		car->wheel[FRNT_LFT].torques.x = 
+		car->wheel->cosax * (steer - car->wheel[FRNT_LFT].steer) * car->wheel[FRNT_LFT].prespinVel * car->wheel[FRNT_LFT].I / SimDeltaTime;
 		car->wheel[FRNT_LFT].steer = steer;
 	} else {
+		car->wheel[FRNT_RGT].torques.x = 
+		car->wheel->cosax * (steer - car->wheel[FRNT_RGT].steer) * car->wheel[FRNT_RGT].prespinVel * car->wheel[FRNT_RGT].I / SimDeltaTime;
 		car->wheel[FRNT_RGT].steer = steer;
+		car->wheel[FRNT_LFT].torques.x = 
+		car->wheel->cosax * (-steer2 - car->wheel[FRNT_LFT].steer) * car->wheel[FRNT_LFT].prespinVel * car->wheel[FRNT_LFT].I / SimDeltaTime;
 		car->wheel[FRNT_LFT].steer = -steer2;
 	}
 }

src/modules/simu/simuv2.1/wheel.cpp

@@ -58,6 +58,8 @@ SimWheelConfig(tCar *car, int index)
 	} else {
 		wheel->relPos.ax = wheel->staticPos.ax;
 	}
+	wheel->cosax = cos(wheel->relPos.ax);
+	wheel->sinax = sin(wheel->relPos.ax);
 
 	wheel->lfMin = MIN(0.8f, wheel->lfMin);
 	wheel->lfMax = MAX(1.6f, wheel->lfMax);
@@ -103,6 +105,7 @@ SimWheelConfig(tCar *car, int index)
 	wheel->feedBack.Tq = 0.0f;
 	wheel->feedBack.brkTq = 0.0f;
 	wheel->rel_vel = 0.0f;
+	wheel->torques.x = wheel->torques.y = wheel->torques.z = 0.0f;
 }
 
 
@@ -276,9 +279,25 @@ SimWheelUpdateRotation(tCar *car)
 {
 	int i;
 	tWheel *wheel;
+	tdble deltan;
+	tdble cosaz, sinaz;
 
 	for (i = 0; i < 4; i++) {
 		wheel = &(car->wheel[i]);
+		/*calculate gyroscopic forces*/
+		cosaz = cos(wheel->relPos.az);
+		sinaz = sin(wheel->relPos.az);
+		if( (i == 0) || (i == 1) ){
+			wheel->torques.y = wheel->torques.x * sinaz;
+			wheel->torques.x = wheel->torques.x * cosaz;
+		} else {
+			wheel->torques.x = wheel->torques.y =0.0;
+		}
+		deltan = -(wheel->in.spinVel - wheel->prespinVel) * wheel->I / SimDeltaTime;
+		wheel->torques.x -= deltan * wheel->cosax *sinaz;
+		wheel->torques.y += deltan * wheel->cosax *cosaz;
+		wheel->torques.z = deltan * wheel->sinax;
+		/*update rotation*/
 		wheel->spinVel = wheel->in.spinVel;
 		FLOAT_RELAXATION2(wheel->spinVel, wheel->prespinVel, 50.0f);
 

src/modules/simu/simuv2.1/wheel.h

@@ -31,6 +31,7 @@ typedef struct
 
     /* dynamic */
     t3Dd	forces;		/* forces acting on car */
+    t3Dd	torques;	/* torques acting on car (gyroscopic forces) */
     tdble	rollRes;	/* Rolling Resistance (summed over the car) */
     tdble	rideHeight;	/* height of the bottom of the car */
     tdble	zRoad;		/* z of the road */
@@ -56,6 +57,7 @@ typedef struct
     /* static */
     tPosd	staticPos;	/* pos relative to the GC (z is suspension travel at rest) */
 				/* and angles are camber (ax), caster (ay) and toe (az) */
+    tdble   cosax, sinax;/*cosinus and sinus of relPos.ax*/
     tdble	rollCenter;
 
     tdble  	weight0;	/* initial weight on this wheel */