void Body::addLocalForce( Ogre::Vector3& force, Ogre::Vector3& pos )
{
Ogre::Vector3 bodypos;
Ogre::Quaternion bodyorient;
getPositionOrientation( bodyorient, bodypos );
Ogre::Vector3 globalforce = bodyorient * force;
Ogre::Vector3 globalpoint = (bodyorient * pos) + bodypos;
addGlobalForce( globalforce, globalpoint );
}
void ossimAlphaSensorHSI::worldToLineSample(const ossimGpt& world_point,
ossimDpt& image_point) const
{
// Initialize at middle
ossim_float64 refL = theImageSize.y/2;
ossim_float64 drefL = 5;
int nIter = 0;
ossimColumnVector3d camLOS;
ossimColumnVector3d camLOS1;
// Iterate using Newton's method
while (nIter<3)
{
ossim_float64 Fx[2];
ossim_float64 refl[2];
refl[0] = refL;
refl[1] = refL + drefL;
for (int ll=0; ll<2; ++ll)
{
// Compute camera position & orientation matrix
ossimEcefPoint platPos;
NEWMAT::Matrix cam2EcfRot;
getPositionOrientation(refl[ll], platPos, cam2EcfRot);
// Compute ECF vector
ossimEcefPoint worldPointECF = ossimEcefPoint(world_point);
ossimColumnVector3d ecfLOS = worldPointECF.data() - platPos.data();
// Rotate to camera frame
camLOS = cam2EcfRot.t() * ecfLOS;
if (ll==0)
camLOS1 = camLOS;
// Set function value
ossim_float64 cScanAngle = atan(camLOS[1]/camLOS[2]);
ossim_float64 scanAngle = getScanAngle(refl[ll]);
Fx[ll] = cScanAngle - scanAngle;
}
// Compute numeric 1st derivative & new estimate for reference line (refL)
ossim_float64 dFx = (Fx[1]-Fx[0]) / drefL;
refL -= Fx[0]/dFx;
nIter++;
}
ossim_float64 samp = m_adjustedFocalLength*camLOS1[0]/camLOS1[2] + theImageSize.x/2;
ossimDpt p(samp, refL);
image_point = p;
}
void Body::addGlobalForce( Ogre::Vector3& force, Ogre::Vector3& pos )
{
Ogre::Vector3 bodypos;
Ogre::Quaternion bodyorient;
getPositionOrientation( bodyorient, bodypos );
Ogre::Vector3 topoint = pos - bodypos;
Ogre::Vector3 torque = topoint.crossProduct( force );
addForce( force );
addTorque( torque );
}
void ossimAlphaSensorHSI::imagingRay(const ossimDpt& imagePoint,
ossimEcefRay& imageRay) const
{
ossim_float64 line = imagePoint.y;
// Form camera frame LOS vector
ossim_float64 scanAngle = getScanAngle(line);
ossimColumnVector3d camLOS(imagePoint.x - theImageSize.x/2,
m_adjustedFocalLength * tan(scanAngle),
m_adjustedFocalLength);
// Compute camera position & orientation matrix
ossimEcefPoint platPos;
NEWMAT::Matrix cam2EcfRot;
getPositionOrientation(line, platPos, cam2EcfRot);
// Rotate camera vector to ECF
ossimColumnVector3d ecfLOS = cam2EcfRot * camLOS.unit();
// Construct ECF image ray
imageRay.setOrigin(platPos);
ossimEcefVector ecfRayDir(ecfLOS);
imageRay.setDirection(ecfRayDir);
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "ossimAlphaSensorHSI::imagingRay DEBUG:\n"
<< " imagePoint = " << imagePoint << "\n"
<< " imageRay = " << imageRay << "\n"
<< " camLOS = " << camLOS << "\n"
<< " platPos = " << platPos << "\n"
<< std::endl;
}
}
