[Csp-commits] APPLICATIONS/CSPSim/Source AircraftPhysicModel.cpp,1.1,1.2

[<mk...@us...>]

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

Modified Files:
	AircraftPhysicModel.cpp 
Log Message:


Index: AircraftPhysicModel.cpp
===================================================================
RCS file: /cvsroot/csp/APPLICATIONS/CSPSim/Source/AircraftPhysicModel.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** AircraftPhysicModel.cpp	19 Jun 2003 18:06:29 -0000	1.1
--- AircraftPhysicModel.cpp	20 Jun 2003 21:24:28 -0000	1.2
***************
*** 38,48 ****
  
  	// 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]);
  	m_AngularVelocityBody = simdata::Vector3(y[6],y[7],y[8]);
  	m_qOrientation = simdata::Quaternion(y[9],y[10],y[11],y[12]);
  	double mag = m_qOrientation.Magnitude();
! 	if (mag != 0.0) 
  		m_qOrientation /= mag;
  	y[9]  = m_qOrientation.w; y[10] = m_qOrientation.x; y[11] = m_qOrientation.y; y[12] = m_qOrientation.z;
  
--- 38,53 ----
  
  	// bind(y,p,v,w,q); 
! 	// XXX the following line is incorrect.
! 	//m_PositionLocal = *m_Position + bodyToLocal(simdata::Vector3(y[0],y[1],y[2]));
! 	// XXX if, in the future, we need to access the local position here, it should be 
! 	// stored in a temporary:
! 	// simdata::Vecter3 position = m_LocalPositon + bodyToLocal(simdata::Vector3(y[0],y[1],y[2]));
  	m_VelocityBody = simdata::Vector3(y[3],y[4],y[5]);
  	m_AngularVelocityBody = simdata::Vector3(y[6],y[7],y[8]);
  	m_qOrientation = simdata::Quaternion(y[9],y[10],y[11],y[12]);
  	double mag = m_qOrientation.Magnitude();
! 	if (mag != 0.0) {
  		m_qOrientation /= mag;
+ 	}
  	y[9]  = m_qOrientation.w; y[10] = m_qOrientation.x; y[11] = m_qOrientation.y; y[12] = m_qOrientation.z;
  
***************
*** 66,70 ****
  	for (;bd != bdEnd; ++bd) {
  		(*bd)->update(x);
! 		m_ForcesBody += (*bd)->getForce();
  		m_MomentsBody += (*bd)->getMoment();
  	}
--- 71,76 ----
  	for (;bd != bdEnd; ++bd) {
  		(*bd)->update(x);
! 		simdata::Vector3 force = (*bd)->getForce();
! 		m_ForcesBody += force;
  		m_MomentsBody += (*bd)->getMoment();
  	}
***************
*** 78,82 ****
  	// angular acceleration body: calculate Iw' = M - w^Iw
  	m_AngularAccelBody = m_InertiaInverse * 
! 				                  (m_MomentsBody - (m_AngularVelocityBody^(m_Inertia * m_AngularVelocityBody)));
  	
  	// quaternion derivative and w in body coordinates: q' = 0.5 * q * w
--- 84,88 ----
  	// angular acceleration body: calculate Iw' = M - w^Iw
  	m_AngularAccelBody = m_InertiaInverse * 
! 	                     (m_MomentsBody - (m_AngularVelocityBody^(m_Inertia * m_AngularVelocityBody)));
  	
  	// quaternion derivative and w in body coordinates: q' = 0.5 * q * w
***************
*** 96,102 ****
  
  void AircraftPhysicModel::doSimStep(double dt) {	
! 	if (dt == 0.0) 
! 		dt = 0.017;
! 	unsigned short const n = std::min<short>(2,static_cast<unsigned short>(180 * dt)) + 1;
  	double dtlocal = dt/n;
  
--- 102,107 ----
  
  void AircraftPhysicModel::doSimStep(double dt) {	
! 	if (dt == 0.0) dt = 0.017;
! 	unsigned short n = std::min<short>(6,static_cast<unsigned short>(180 * dt)) + 1;
  	double dtlocal = dt/n;
  
***************
*** 105,120 ****
  	Atmosphere const *atmosphere = CSPSim::theSim->getAtmosphere();
  	if (atmosphere)
! 		m_Gravity =	atmosphere->getGravity(m_PositionLocal.z);
  	else
! 		m_Gravity =	9.806;
  
! 	m_WeightLocal = - m_Mass *	m_Gravity *	simdata::Vector3::ZAXIS;
  
! 	for	(unsigned short	i = 0; i<n; ++i) {
  
! 		m_PositionLocal	= *m_Position;
! 		m_VelocityLocal	= *m_Velocity;
! 		m_AngularVelocityLocal = *m_AngularVelocity;
! 		m_qOrientation = *m_Orientation;
  
  		m_VelocityBody = localToBody(m_VelocityLocal);
--- 110,125 ----
  	Atmosphere const *atmosphere = CSPSim::theSim->getAtmosphere();
  	if (atmosphere)
! 		m_Gravity = atmosphere->getGravity(m_PositionLocal.z);
  	else
! 		m_Gravity = 9.806;
  
! 	m_WeightLocal = - m_Mass * m_Gravity * simdata::Vector3::ZAXIS;
  
! 	m_PositionLocal	= *m_Position;
! 	m_VelocityLocal	= *m_Velocity;
! 	m_AngularVelocityLocal = *m_AngularVelocity;
! 	m_qOrientation = *m_Orientation;
  
! 	for (unsigned short i = 0; i<n; ++i) {
  
  		m_VelocityBody = localToBody(m_VelocityLocal);
***************
*** 123,134 ****
  		updateNearGround();
  
! 		std::vector<double>	y0 = bodyToY(simdata::Vector3::ZERO,m_VelocityBody,m_AngularVelocityBody,m_qOrientation);
  
  		std::for_each(m_Dynamics.begin(),m_Dynamics.end(),PreSimulationStep(dtlocal));
  
  		// solution
! 		std::vector<double>	y =	flow(y0, 0,	dtlocal);
  		// update all variables
! 		// Caution:	don	t permute these	lines
  		// solution to body data members
  		YToBody(y);
--- 128,139 ----
  		updateNearGround();
  
! 		std::vector<double> y0 = bodyToY(simdata::Vector3::ZERO,m_VelocityBody,m_AngularVelocityBody,m_qOrientation);
  
  		std::for_each(m_Dynamics.begin(),m_Dynamics.end(),PreSimulationStep(dtlocal));
  
  		// solution
! 		std::vector<double> y = flow(y0, 0, dtlocal);
  		// update all variables
! 		// Caution: dont permute these	lines
  		// solution to body data members
  		YToBody(y);
***************
*** 146,157 ****
  		double mag = m_qOrientation.Magnitude();
  		if (mag	!= 0.0)
! 			m_qOrientation /=	mag;
  	}
  	// returns vehicle data members
! 	*m_Position	= m_PositionLocal;
! 	if (m_GroundCollisionDynamics->hasContact() && m_VelocityLocal.z < 0.0)
  		m_VelocityLocal.z *= 0.99;
! 	*m_Velocity	= m_VelocityLocal;
  	*m_AngularVelocity = m_AngularVelocityLocal;
  	*m_Orientation = m_qOrientation;	
! }
\ No newline at end of file
--- 151,164 ----
  		double mag = m_qOrientation.Magnitude();
  		if (mag	!= 0.0)
! 			m_qOrientation /= mag;
  	}
+ 
  	// returns vehicle data members
! 	*m_Position = m_PositionLocal;
! 	if (m_GroundCollisionDynamics->hasContact() && m_VelocityLocal.z < 0.0) {
  		m_VelocityLocal.z *= 0.99;
! 	}
! 	*m_Velocity = m_VelocityLocal;
  	*m_AngularVelocity = m_AngularVelocityLocal;
  	*m_Orientation = m_qOrientation;	
! }