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