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;
! }
|
