From aebb440e5bbdfa15edb225b8cde634ac26da45b6 Mon Sep 17 00:00:00 2001
From: kakukri <kakukri@30fe4595-0a0c-4342-8851-515496e4dcbd>
Date: Wed, 31 Jul 2013 20:13:14 +0000
Subject: [PATCH] Heave spring: add speed for damper, better distribute initial
 weight

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

Former-commit-id: 53e2f0579871044b4f0d926da06449809990abdf
Former-commit-id: 74f5c60a8e1fa9f0d9b33f32c731647ced63ca9d
---
 src/modules/simu/simuv4/axle.cpp | 11 +++++++----
 src/modules/simu/simuv4/car.cpp  | 28 +++++++++++++++++++---------
 src/modules/simu/simuv4/sim.h    |  2 +-
 3 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/modules/simu/simuv4/axle.cpp b/src/modules/simu/simuv4/axle.cpp
index d0a2f67ac..0cecfedc1 100644
--- a/src/modules/simu/simuv4/axle.cpp
+++ b/src/modules/simu/simuv4/axle.cpp
@@ -22,7 +22,7 @@
 static const char *AxleSect[2] = {SECT_FRNTAXLE, SECT_REARAXLE};
 static const char *WheelSect[4] = {SECT_FRNTRGTWHEEL, SECT_FRNTLFTWHEEL, SECT_REARRGTWHEEL, SECT_REARLFTWHEEL};
 
-void SimAxleConfig(tCar *car, int index)
+void SimAxleConfig(tCar *car, int index, tdble weight0)
 {
 	void	*hdle = car->params;
 	tdble	rollCenter, x0r, x0l;
@@ -39,11 +39,11 @@ void SimAxleConfig(tCar *car, int index)
 	if (index == 0) {
 		SimSuspConfig(hdle, SECT_FRNTARB, &(axle->arbSusp), 0, 0);
 		axle->arbSusp.spring.K = -axle->arbSusp.spring.K;
-		SimSuspConfig(hdle, SECT_FRNTHEAVE, &(axle->heaveSusp), 0.0, 0.5*(x0r+x0l));
+		SimSuspConfig(hdle, SECT_FRNTHEAVE, &(axle->heaveSusp), weight0, 0.5*(x0r+x0l));
 	} else {
 		SimSuspConfig(hdle, SECT_REARARB, &(axle->arbSusp), 0, 0);
 		axle->arbSusp.spring.K = -axle->arbSusp.spring.K;
-		SimSuspConfig(hdle, SECT_REARHEAVE, &(axle->heaveSusp), 0.0, 0.5*(x0r+x0l));
+		SimSuspConfig(hdle, SECT_REARHEAVE, &(axle->heaveSusp), weight0, 0.5*(x0r+x0l));
 	}
 	
 	car->wheel[index*2].feedBack.I += (tdble) (axle->I / 2.0);
@@ -56,10 +56,12 @@ void SimAxleConfig(tCar *car, int index)
 void SimAxleUpdate(tCar *car, int index)
 {
 	tAxle *axle = &(car->axle[index]);
-	tdble str, stl, sgn;
+	tdble str, stl, sgn, vtl, vtr;
 	
 	str = car->wheel[index*2].susp.x;
 	stl = car->wheel[index*2+1].susp.x;
+	vtr = car->wheel[index*2].susp.v;
+	vtl = car->wheel[index*2+1].susp.v;
 	
 	sgn = (tdble) (SIGN(stl - str));
 	axle->arbSusp.x = fabs(stl - str);		
@@ -77,6 +79,7 @@ void SimAxleUpdate(tCar *car, int index)
 	
 	/* heave/center spring */
 	axle->heaveSusp.x = 0.5 * (stl + str);
+	axle->heaveSusp.v = 0.5 * (vtl + vtr);
 	SimSuspUpdate(&(axle->heaveSusp));
 	f = 0.5 * axle->heaveSusp.force;
 	car->wheel[index*2].axleFz += f;
diff --git a/src/modules/simu/simuv4/car.cpp b/src/modules/simu/simuv4/car.cpp
index 79e4d3b41..5b5b27723 100644
--- a/src/modules/simu/simuv4/car.cpp
+++ b/src/modules/simu/simuv4/car.cpp
@@ -25,6 +25,7 @@
 #include "sim.h"
 
 const tdble aMax = 1.0f; /*  */
+static const char *SuspSect[4] = {SECT_FRNTRGTSUSP, SECT_FRNTLFTSUSP, SECT_REARRGTSUSP, SECT_REARLFTSUSP};
 
 void
 SimCarConfig(tCar *car)
@@ -94,25 +95,34 @@ SimCarConfig(tCar *car)
 	car->Iinv.z = (tdble) (12.0 / (car->mass * k * (car->dimension.y * car->dimension.y + car->dimension.x * car->dimension.x)));
 	
 	/* configure components */
+	tdble K[4], Kfheave, Krheave;
+	for (i = 0; i < 4; i++) {
+		K[i] = GfParmGetNum(hdle, SuspSect[i], PRM_SPR, (char*)NULL, 175000.0f);
+	}
+	Kfheave = GfParmGetNum(hdle, SECT_FRNTHEAVE, PRM_SPR, (char*)NULL, 175000.0f);
+	Krheave = GfParmGetNum(hdle, SECT_REARHEAVE, PRM_SPR, (char*)NULL, 175000.0f);
 	w = car->mass * G;
 	
 	wf0 = w * gcfr;
 	wr0 = w * (1 - gcfr);
 	
-	car->wheel[FRNT_RGT].weight0 = wf0 * gcfrl;
-	car->wheel[FRNT_LFT].weight0 = wf0 * (1 - gcfrl);
-	car->wheel[REAR_RGT].weight0 = wr0 * gcrrl;
-	car->wheel[REAR_LFT].weight0 = wr0 * (1 - gcrrl);
+	car->wheel[FRNT_RGT].weight0 = wf0 * gcfrl * K[FRNT_RGT] / (K[FRNT_RGT] + 0.5*Kfheave);
+	car->wheel[FRNT_LFT].weight0 = wf0 * (1 - gcfrl) * K[FRNT_LFT] / (K[FRNT_LFT] + 0.5*Kfheave);
+	car->wheel[REAR_RGT].weight0 = wr0 * gcrrl * K[REAR_RGT] / (K[REAR_RGT] + 0.5*Krheave);
+	car->wheel[REAR_LFT].weight0 = wr0 * (1 - gcrrl) * K[REAR_LFT] / (K[REAR_LFT] + 0.5*Krheave);
 	
-	for (i = 0; i < 2; i++) {
-		SimAxleConfig(car, i);
-	}
+	/*for (i = 0; i < 2; i++) {
+		SimAxleConfig(car, i, 0.0);
+	}*/
+	wf0 = (wf0 - car->wheel[FRNT_RGT].weight0 - car->wheel[FRNT_LFT].weight0);
+	wr0 = (wr0 - car->wheel[REAR_RGT].weight0 - car->wheel[REAR_LFT].weight0);
+	SimAxleConfig(car, FRNT, wf0);
+	SimAxleConfig(car, REAR, wr0);
 	
 	for (i = 0; i < 4; i++) {
 		SimWheelConfig(car, i); 
 	}
-	
-	
+
 	SimEngineConfig(car);
 	SimTransmissionConfig(car);
 	SimSteerConfig(car);
diff --git a/src/modules/simu/simuv4/sim.h b/src/modules/simu/simuv4/sim.h
index 38790a2ec..fbd6fd798 100644
--- a/src/modules/simu/simuv4/sim.h
+++ b/src/modules/simu/simuv4/sim.h
@@ -46,7 +46,7 @@ extern void SimUpdateSingleCar(int index, double deltaTime,tSituation *s);
 extern tDynPt* GetSimCarTable(int index);
 
 
-extern void SimAxleConfig(tCar *car, int index);
+extern void SimAxleConfig(tCar *car, int index, tdble weight0);
 extern void SimAxleUpdate(tCar *car, int index);
 
 extern void SimCarConfig(tCar *car);