[Csp-commits] APPLICATIONS/CSPSim/Source DynamicalSystem.cpp,1.2,1.3 AircraftObject.cpp,1.11,1.12 AeroDynamics.cpp,1.15,1.16

[<de...@us...>]

Update of /cvsroot/csp/APPLICATIONS/CSPSim/Source
In directory sc8-pr-cvs1:/tmp/cvs-serv2386

Modified Files:
	DynamicalSystem.cpp AircraftObject.cpp AeroDynamics.cpp 
Log Message:
no message

Index: DynamicalSystem.cpp
===================================================================
RCS file: /cvsroot/csp/APPLICATIONS/CSPSim/Source/DynamicalSystem.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** DynamicalSystem.cpp	3 Apr 2003 10:40:42 -0000	1.2
--- DynamicalSystem.cpp	12 Apr 2003 10:17:51 -0000	1.3
***************
*** 56,60 ****
          y = _numericalMethod->enhancedSolve(y0, t0, dt);
  		if (_numericalMethod->failed()) {
! 			std::cout << "quick solve is required\n";
  			y = _numericalMethod->quickSolve(y0, t0, dt);
  		}
--- 56,60 ----
          y = _numericalMethod->enhancedSolve(y0, t0, dt);
  		if (_numericalMethod->failed()) {
! 			//std::cout << "quick solve is required\n";
  			y = _numericalMethod->quickSolve(y0, t0, dt);
  		}

Index: AircraftObject.cpp
===================================================================
RCS file: /cvsroot/csp/APPLICATIONS/CSPSim/Source/AircraftObject.cpp,v
retrieving revision 1.11
retrieving revision 1.12
diff -C2 -d -r1.11 -r1.12
*** AircraftObject.cpp	3 Apr 2003 10:40:42 -0000	1.11
--- AircraftObject.cpp	12 Apr 2003 10:17:51 -0000	1.12
***************
*** 161,166 ****
  	CSP_LOG(CSP_APP, CSP_DEBUG, "AircraftObject::onUpdate ...");
  	// TODO: update AI controller
- 	doFCS(dt);
  	updateControls(dt);
  	DynamicObject::onUpdate(dt);
  	// TODO: update HUD?
--- 161,166 ----
  	CSP_LOG(CSP_APP, CSP_DEBUG, "AircraftObject::onUpdate ...");
  	// TODO: update AI controller
  	updateControls(dt);
+ 	doFCS(dt);
  	DynamicObject::onUpdate(dt);
  	// TODO: update HUD?
***************
*** 176,180 ****
  unsigned int AircraftObject::onRender()
  {
! //	updateScene();
  	return 0;
  }
--- 176,180 ----
  unsigned int AircraftObject::onRender()
  {
! 	// TODO: animation code here
  	return 0;
  }
***************
*** 471,476 ****
  	m_FlightModel->setGroundZ(m_GroundZ);
  	m_FlightModel->setGroundN(m_GroundN);
! 	m_FlightModel->doSimStep(dt);
! 	//m_FlightModel->doSimStep2(dt);
  	updateOrientation();
  	m_Direction = m_Orientation * simdata::Vector3::YAXIS;
--- 471,476 ----
  	m_FlightModel->setGroundZ(m_GroundZ);
  	m_FlightModel->setGroundN(m_GroundN);
! 	//m_FlightModel->doSimStep(dt);
! 	m_FlightModel->doSimStep2(dt);
  	updateOrientation();
  	m_Direction = m_Orientation * simdata::Vector3::YAXIS;

Index: AeroDynamics.cpp
===================================================================
RCS file: /cvsroot/csp/APPLICATIONS/CSPSim/Source/AeroDynamics.cpp,v
retrieving revision 1.15
retrieving revision 1.16
diff -C2 -d -r1.15 -r1.16
*** AeroDynamics.cpp	12 Apr 2003 00:55:03 -0000	1.15
--- AeroDynamics.cpp	12 Apr 2003 10:17:51 -0000	1.16
***************
*** 47,51 ****
  SIMDATA_REGISTER_INTERFACE(AeroDynamics)
  
- 
  AeroDynamics::AeroDynamics(): 
  	DynamicalSystem(13),
--- 47,50 ----
***************
*** 629,633 ****
  	// Add in extra forces and reset
  	forceBody += m_ExtraForceBody;
- 	m_ExtraForceBody = simdata::Vector3(0.0, 0.0, 0.0);
  	
  	// Evaluate g's
--- 628,631 ----
***************
*** 652,656 ****
  
  	moments += m_ExtraMomentBody;
- 	m_ExtraMomentBody = simdata::Vector3(0.0, 0.0, 0.0);
  
  	return moments;
--- 650,653 ----
***************
*** 853,858 ****
  double const damping = 0.91;
  
! double const scaleFactor1 = 0.4;
! double const scaleFactor2 = 1.0;
  
  std::vector<double> const &AeroDynamics::_f(double x, std::vector<double> &y) {   
--- 850,854 ----
  double const damping = 0.91;
  
! double const scaleFactor2 = 0.1;
  
  std::vector<double> const &AeroDynamics::_f(double x, std::vector<double> &y) {   
***************
*** 860,864 ****
  	static std::vector<double> dy(13);
  
! 	// bind(y,p,v,w,q);
  	m_PositionLocal = *m_Position + BodyToLocal(simdata::Vector3(y[0],y[1],y[2]));
  	m_VelocityBody = simdata::Vector3(y[3],y[4],y[5]);
--- 856,860 ----
  	static std::vector<double> dy(13);
  
! 	// bind(y,p,v,w,q); 
  	m_PositionLocal = *m_Position + BodyToLocal(simdata::Vector3(y[0],y[1],y[2]));
  	m_VelocityBody = simdata::Vector3(y[3],y[4],y[5]);
***************
*** 870,874 ****
  	y[9]  = qOrientation.w; y[10] = qOrientation.x; y[11] = qOrientation.y; y[12] = qOrientation.z;
  
- // XXX partial windspeed adaptation... broken for doSimStep2
  	m_AirflowBody = m_VelocityBody - LocalToBody(m_WindLocal);
  	m_AirSpeed = m_AirflowBody.Length();
--- 866,869 ----
***************
*** 879,883 ****
  	updateAngles(x);
  
! 	// linear acceleration body: calculate v' = F/m - w^v
  	if (m_NearGround) {
  		// approximate (generic) ground effect... TODO: paramaterize in xml.
--- 874,879 ----
  	updateAngles(x);
  
! 	m_ExtraForceBody = simdata::Vector3(0.0, 0.0, 0.0);
! 	m_ExtraMomentBody = simdata::Vector3(0.0, 0.0, 0.0);
  	if (m_NearGround) {
  		// approximate (generic) ground effect... TODO: paramaterize in xml.
***************
*** 886,903 ****
  		if (h > 1.0) h = 1.0;
  		m_GE = 0.49 + (1.0 - 0.5 * h) * h;
! 		//if (m_Gear) {
! 		//	double height = m_PositionLocal.z - m_GroundZ;
! 		//	simdata::Vector3 force, moment;
! 		//	m_Gear->doComplexPhysics(qOrientation, 
! 		//		m_VelocityBody, 
! 		//		m_AngularVelocityBody, 
! 		//		height, 
! 		//		m_GroundN,
! 		//		x, 
! 		//		force, 
! 		//		moment);
! 		//	m_ExtraForceBody += scaleFactor1 * force;
! 		//	m_ExtraMomentBody += scaleFactor1 * moment;
! 		//}//if (m_Gear)
  		if (m_Contacts) {
  			simdata::Vector3 bodyn = LocalToBody(m_GroundN);
--- 882,899 ----
  		if (h > 1.0) h = 1.0;
  		m_GE = 0.49 + (1.0 - 0.5 * h) * h;
! 		if (m_Gear) {
! 			double height = m_PositionLocal.z - m_GroundZ;
! 			simdata::Vector3 force, moment;
! 			m_Gear->doComplexPhysics(qOrientation, 
! 				m_VelocityBody, 
! 				m_AngularVelocityBody, 
! 				height, 
! 				m_GroundN,
! 				x, 
! 				force, 
! 				moment);
! 			m_ExtraForceBody += force;
! 			m_ExtraMomentBody += moment;
! 		}//if (m_Gear)
  		if (m_Contacts) {
  			simdata::Vector3 bodyn = LocalToBody(m_GroundN);
***************
*** 913,918 ****
  					simdata::Vector3 Vb = m_VelocityBody + (m_AngularVelocityBody^(*contact));
  					simdata::Vector3 V = BodyToLocal(Vb);
! 					static const float groundK = 1.0e+4;//1.0e+7;
! 					static const float groundBeta = 1.0e+3;//1.0e+6;
  					double impact = simdata::Dot(V, m_GroundN);
  					double scale = - (height*groundK + impact*fabs(impact)*groundBeta);
--- 909,914 ----
  					simdata::Vector3 Vb = m_VelocityBody + (m_AngularVelocityBody^(*contact));
  					simdata::Vector3 V = BodyToLocal(Vb);
! 					static const float groundK = 1.0e+7;
! 					static const float groundBeta = 1.0e+6;
  					double impact = simdata::Dot(V, m_GroundN);
  					double scale = - (height*groundK + impact*fabs(impact)*groundBeta);
***************
*** 920,929 ****
  						// dissipate some extra energy
  						if (impact < -10.0) {
! 							//m_VelocityBody *= 0.50;
! 							m_VelocityBody -= 0.50 * impact * m_GroundN;
  							std::cout << "SLAM!!!!\n";
  						} else {
! 							//m_VelocityBody *= 0.95;
! 							m_VelocityBody -= 0.95 * impact * m_GroundN;
  							std::cout << "SLAM!\n";
  						}
--- 916,923 ----
  						// dissipate some extra energy
  						if (impact < -10.0) {
! 							m_VelocityBody -= 0.25 * simdata::Dot(m_VelocityBody, bodyn) * bodyn;
  							std::cout << "SLAM!!!!\n";
  						} else {
! 							m_VelocityBody -= 0.05 * simdata::Dot(m_VelocityBody, bodyn) * bodyn;
  							std::cout << "SLAM!\n";
  						}
***************
*** 934,938 ****
  						}
  					}//if (fabs(scale)>groundK)
! 					simdata::Vector3 force = scale * bodyn;
  
  					V -= impact * m_GroundN;
--- 928,933 ----
  						}
  					}//if (fabs(scale)>groundK)
! 					
! 					simdata::Vector3 force = scaleFactor2 * scale * bodyn;
  
  					V -= impact * m_GroundN;
***************
*** 942,963 ****
  					// reduce to zero as speed drops
  					float v = V.Length();
! 					scale = scale / (1.0 + v);
  					// limit to 4G max
  					if (scale * v > m_Mass * 40.0) {
  						scale = m_Mass * 40.0 / v;
  					}
! 					force += -scale * V;
  
! 					m_ExtraForceBody += scaleFactor2 * force;
! 					m_ExtraMomentBody += scaleFactor2 * (*contact) ^ force;
  				} //if (height < 0.0)
  			} //for (contact
  		} //if (m_Contacts)
  	}
  	m_ForcesBody = CalculateForces(qBarS);
  	m_LinearAccelBody =  m_MassInverse * m_ForcesBody - (m_AngularVelocityBody^m_VelocityBody);
  	
- 	// angular acceleration body: calculate Iw' = M - w^Iw
  	m_MomentsBody = CalculateMoments(qBarS);
  	m_AngularAccelBody = m_InertiaInverse * 
  				                  (m_MomentsBody - (m_AngularVelocityBody^(m_Inertia * m_AngularVelocityBody)));
--- 937,961 ----
  					// reduce to zero as speed drops
  					float v = V.Length();
! 					scale /= 1.0 + v;
  					// limit to 4G max
  					if (scale * v > m_Mass * 40.0) {
  						scale = m_Mass * 40.0 / v;
  					}
! 					force += -scaleFactor2 * scale * V;
  
! 					m_ExtraForceBody += force;
! 					m_ExtraMomentBody += (*contact) ^ force;
! 					
  				} //if (height < 0.0)
  			} //for (contact
  		} //if (m_Contacts)
  	}
+ 
  	m_ForcesBody = CalculateForces(qBarS);
+ 	// linear acceleration body: calculate v' = F/m - w^v
  	m_LinearAccelBody =  m_MassInverse * m_ForcesBody - (m_AngularVelocityBody^m_VelocityBody);
  	
  	m_MomentsBody = CalculateMoments(qBarS);
+ 	// angular acceleration body: calculate Iw' = M - w^Iw
  	m_AngularAccelBody = m_InertiaInverse * 
  				                  (m_MomentsBody - (m_AngularVelocityBody^(m_Inertia * m_AngularVelocityBody)));
***************
*** 968,973 ****
--- 966,974 ----
  	// p' = v
  	dy[0] = y[3]; dy[1] = y[4]; dy[2] = y[5];
+ 	// v'
  	dy[3] = m_LinearAccelBody.x; dy[4] = m_LinearAccelBody.y; dy[5] = m_LinearAccelBody.z;
+ 	// w'
  	dy[6] = m_AngularAccelBody.x; dy[7] = m_AngularAccelBody.y; dy[8] = m_AngularAccelBody.z;
+ 	// q'
  	dy[9]  = qprim.w; dy[10] = qprim.x; dy[11] = qprim.y; dy[12] = qprim.z;
  	
***************
*** 1001,1006 ****
  
  void AeroDynamics::doSimStep2(double dt) {  
- 	static std::vector<double> y0, y;
- 
  	if (dt == 0.0) dt = 0.01;
  	
--- 1002,1005 ----
***************
*** 1042,1049 ****
  	m_AngularVelocityBody = LocalToBody(m_AngularVelocityLocal);
  	updateAngles(dt);
! 	y0 = ::bind(simdata::Vector3::ZERO, m_VelocityBody, m_AngularVelocityBody, qOrientation);
  	
  	// solution
! 	y = flow(y0, 0, dt);
  
  	// update all variables
--- 1041,1048 ----
  	m_AngularVelocityBody = LocalToBody(m_AngularVelocityLocal);
  	updateAngles(dt);
! 	std::vector<double> y0 = ::bind(simdata::Vector3::ZERO, m_VelocityBody, m_AngularVelocityBody, qOrientation);
  	
  	// solution
! 	std::vector<double> y = flow(y0, 0, dt);
  
  	// update all variables