Ajout fichier .cpp

Utilities/otbossim/src/ossim/base/ossimQuaternion.cpp

+#include <ossim/base/ossimQuaternion.h>
+#include <ossim/base/ossimMatrix4x4.h>
+/// Good introductions to Quaternions at:
+/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
+/// http://mathworld.wolfram.com/Quaternion.html
+
+void ossim::Quaternion::set(const ossimMatrix4x4& matrix)
+{
+   matrix.getRotate(*this);
+}
+
+void ossim::Quaternion::get(ossimMatrix4x4& matrix) const
+{
+   matrix.makeRotate(*this);
+}
+
+/// Set the elements of the Quat to represent a rotation of angle
+/// (radians) around the axis (x,y,z)
+void ossim::Quaternion::makeRotate( value_type angle, value_type x, value_type y, value_type z )
+{
+   
+   const value_type epsilon = 0.0000001;
+   
+   value_type length = sqrt( x*x + y*y + z*z );
+   if (length < epsilon)
+   {
+      // ~zero length axis, so reset rotation to zero.
+      *this = ossim::Quaternion();
+        return;
+    }
+   double tempAngle = ossim::degreesToRadians(angle);
+   
+   value_type inversenorm  = 1.0/length;
+   value_type coshalfangle = cos( 0.5*tempAngle );
+   value_type sinhalfangle = sin( 0.5*tempAngle );
+   
+   theVector[0] = x * sinhalfangle * inversenorm;
+   theVector[1] = y * sinhalfangle * inversenorm;
+   theVector[2] = z * sinhalfangle * inversenorm;
+   theVector[3] = coshalfangle;
+}
+
+void ossim::Quaternion::makeRotate ( value_type angle1, const ossimColumnVector3d& axis1, 
+                                     value_type angle2, const ossimColumnVector3d& axis2,
+                                     value_type angle3, const ossimColumnVector3d& axis3)
+{
+    ossim::Quaternion q1; q1.makeRotate(angle1,axis1);
+    ossim::Quaternion q2; q2.makeRotate(angle2,axis2);
+    ossim::Quaternion q3; q3.makeRotate(angle3,axis3);
+    *this = q1*q2*q3;
+}                        
+
+void ossim::Quaternion::makeRotate ( value_type angle1, const ossimDpt3d& axis1, 
+                                     value_type angle2, const ossimDpt3d& axis2,
+                                     value_type angle3, const ossimDpt3d& axis3)
+{
+   ossim::Quaternion q1; q1.makeRotate(angle1,axis1);
+   ossim::Quaternion q2; q2.makeRotate(angle2,axis2);
+   ossim::Quaternion q3; q3.makeRotate(angle3,axis3);
+
+    *this = q1*q2*q3;
+}                        
+
+void ossim::Quaternion::makeRotate( const ossimColumnVector3d& vec1, const ossimColumnVector3d& vec2 )
+{
+   makeRotate(ossimDpt3d(vec1[0], vec1[1], vec1[2]),
+              ossimDpt3d(vec2[0], vec2[1], vec2[2]));
+}
+
+
+// /** Make a rotation Quat which will rotate vec1 to vec2
+
+// This routine uses only fast geometric transforms, without costly acos/sin computations. 
+// It's exact, fast, and with less degenerate cases than the acos/sin method.
+
+// For an explanation of the math used, you may see for example: 
+// http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
+
+// @note This is the rotation with shortest angle, which is the one equivalent to the 
+// acos/sin transform method. Other rotations exists, for example to additionally keep 
+// a local horizontal attitude.
+
+// @author Nicolas Brodu
+// */
+void ossim::Quaternion::makeRotate( const ossimDpt3d& from, const ossimDpt3d& to )
+{
+
+    // This routine takes any vector as argument but normalized 
+    // vectors are necessary, if only for computing the dot product.
+    // Too bad the API is that generic, it leads to performance loss.
+    // Even in the case the 2 vectors are not normalized but same length,
+    // the sqrt could be shared, but we have no way to know beforehand
+    // at this point, while the caller may know.
+    // So, we have to test... in the hope of saving at least a sqrt
+    ossimDpt3d sourceVector = from;
+    ossimDpt3d targetVector = to;
+    
+    value_type fromLen2 = from.length2();
+    value_type fromLen = 1.0;
+    // normalize only when necessary, epsilon test
+    if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7))
+    {
+        fromLen = sqrt(fromLen2);
+        sourceVector /= fromLen;
+    }
+    
+    value_type toLen2 = to.length2();
+    // normalize only when necessary, epsilon test
+    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7))
+    {
+        value_type toLen;
+        // re-use fromLen for case of mapping 2 vectors of the same length
+        if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7))
+        {
+            toLen = fromLen;
+        } 
+        else toLen = sqrt(toLen2);
+        targetVector /= toLen;
+    }
+    
+    // Now let's get into the real stuff
+    // Use "dot product plus one" as test as it can be re-used later on
+    double dotProdPlus1 = 1.0 + sourceVector * targetVector;
+    
+    // Check for degenerate case of full u-turn. Use epsilon for detection
+    if (dotProdPlus1 < 1e-7) {
+    
+        // Get an orthogonal vector of the given vector
+        // in a plane with maximum vector coordinates.
+        // Then use it as quaternion axis with pi angle
+        // Trick is to realize one value at least is >0.6 for a normalized vector.
+        if (fabs(sourceVector.x) < 0.6) {
+            const double norm = sqrt(1.0 - sourceVector.x * sourceVector.x);
+            theVector[0] = 0.0; 
+            theVector[1] = sourceVector.z / norm;
+            theVector[2] = -sourceVector.y / norm;
+            theVector[3] = 0.0;
+        } else if (fabs(sourceVector.y) < 0.6) {
+            const double norm = sqrt(1.0 - sourceVector.y * sourceVector.y);
+            theVector[0] = -sourceVector.z / norm;
+            theVector[1] = 0.0;
+            theVector[2] = sourceVector.x / norm;
+            theVector[3] = 0.0;
+        } else {
+            const double norm = sqrt(1.0 - sourceVector.z * sourceVector.z);
+            theVector[0] = sourceVector.y / norm;
+            theVector[1] = -sourceVector.x / norm;
+            theVector[2] = 0.0;
+            theVector[3] = 0.0;
+        }
+    }
+    
+    else {
+        // Find the shortest angle quaternion that transforms normalized vectors
+        // into one other. Formula is still valid when vectors are colinear
+        const double s = sqrt(0.5 * dotProdPlus1);
+        const ossimDpt3d tmp = sourceVector ^ targetVector / (2.0*s);
+        theVector[0] = tmp.x;
+        theVector[1] = tmp.y;
+        theVector[2] = tmp.z;
+        theVector[3] = s;
+    }
+}
+
+
+// Make a rotation Quat which will rotate vec1 to vec2
+// Generally take adot product to get the angle between these
+// and then use a cross product to get the rotation axis
+// Watch out for the two special cases of when the vectors
+// are co-incident or opposite in direction.
+// void ossim::Quaternion::makeRotate_original( const Vec3d& from, const Vec3d& to )
+// {
+//     const value_type epsilon = 0.0000001;
+
+//     value_type length1  = from.length();
+//     value_type length2  = to.length();
+    
+//     // dot product vec1*vec2
+//     value_type cosangle = from*to/(length1*length2);
+
+//     if ( fabs(cosangle - 1) < epsilon )
+//     {
+//         osg::notify(osg::INFO)<<"*** ossim::Quaternion::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
+    
+//         // cosangle is close to 1, so the vectors are close to being coincident
+//         // Need to generate an angle of zero with any vector we like
+//         // We'll choose (1,0,0)
+//         makeRotate( 0.0, 0.0, 0.0, 1.0 );
+//     }
+//     else
+//     if ( fabs(cosangle + 1.0) < epsilon )
+//     {
+//         // vectors are close to being opposite, so will need to find a
+//         // vector orthongonal to from to rotate about.
+//         Vec3d tmp;
+//         if (fabs(from.x())<fabs(from.y()))
+//             if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
+//             else tmp.set(0.0,0.0,1.0);
+//         else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
+//         else tmp.set(0.0,0.0,1.0);
+        
+//         Vec3d fromd(from.x(),from.y(),from.z());
+        
+//         // find orthogonal axis.
+//         Vec3d axis(fromd^tmp);
+//         axis.normalize();
+        
+//         theVector[0] = axis[0]; // sin of half angle of PI is 1.0.
+//         theVector[1] = axis[1]; // sin of half angle of PI is 1.0.
+//         theVector[2] = axis[2]; // sin of half angle of PI is 1.0.
+//         theVector[3] = 0; // cos of half angle of PI is zero.
+
+//     }
+//     else
+//     {
+//         // This is the usual situation - take a cross-product of vec1 and vec2
+//         // and that is the axis around which to rotate.
+//         Vec3d axis(from^to);
+//         value_type angle = acos( cosangle );
+//         makeRotate( angle, axis );
+//     }
+// }
+
+// void ossim::Quaternion::getRotate( value_type& angle, Vec3f& vec ) const
+// {
+//     value_type x,y,z;
+//     getRotate(angle,x,y,z);
+//     vec[0]=x;
+//     vec[1]=y;
+//     vec[2]=z;
+// }
+// void ossim::Quaternion::getRotate( value_type& angle, Vec3d& vec ) const
+// {
+//     value_type x,y,z;
+//     getRotate(angle,x,y,z);
+//     vec[0]=x;
+//     vec[1]=y;
+//     vec[2]=z;
+// }
+
+
+// Get the angle of rotation and axis of this Quat object.
+// Won't give very meaningful results if the Quat is not associated
+// with a rotation!
+void ossim::Quaternion::getRotate( value_type& angle, value_type& x, value_type& y, value_type& z ) const
+{
+    value_type sinhalfangle = sqrt( theVector[0]*theVector[0] + theVector[1]*theVector[1] + theVector[2]*theVector[2] );
+
+    angle = 2.0 * atan2( sinhalfangle, theVector[3] );
+    if(sinhalfangle)
+    {
+        x = theVector[0] / sinhalfangle;
+        y = theVector[1] / sinhalfangle;
+        z = theVector[2] / sinhalfangle;
+    }
+    else
+    {
+        x = 0.0;
+        y = 0.0;
+        z = 1.0;
+    }
+    angle = ossim::radiansToDegrees(angle);
+}
+
+#if 0
+/// Spherical Linear Interpolation
+/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
+/// Reference: Shoemake at SIGGRAPH 89
+/// See also
+/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
+void ossim::Quaternion::slerp( value_type t, const ossim::Quaternion& from, const ossim::Quaternion& to )
+{
+    const double epsilon = 0.00001;
+    double omega, cosomega, sinomega, scale_from, scale_to ;
+    
+    osg::Quat quatTo(to);
+    // this is a dot product
+    
+    cosomega = from.asVec4() * to.asVec4();
+    
+    if ( cosomega <0.0 )
+    { 
+        cosomega = -cosomega; 
+        quatTo = -to;
+    }
+
+    if( (1.0 - cosomega) > epsilon )
+    {
+        omega= acos(cosomega) ;  // 0 <= omega <= Pi (see man acos)
+        sinomega = sin(omega) ;  // this sinomega should always be +ve so
+        // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
+        scale_from = sin((1.0-t)*omega)/sinomega ;
+        scale_to = sin(t*omega)/sinomega ;
+    }
+    else
+    {
+        /* --------------------------------------------------
+           The ends of the vectors are very close
+           we can use simple linear interpolation - no need
+           to worry about the "spherical" interpolation
+           -------------------------------------------------- */
+        scale_from = 1.0 - t ;
+        scale_to = t ;
+    }
+
+    *this = (from*scale_from) + (quatTo*scale_to);
+    
+    // so that we get a Vec4
+}
+#endif

Utilities/otbossim/src/ossim/imaging/ossimJpegMemSrc.cpp

+//----------------------------------------------------------------------------
+// License:  See top level LICENSE.txt file.
+//
+// Author:  David Burken, original code from Thomas G. Lane (memsrc.c)
+//
+// Description:
+// Code to use jpeg-6b library to read jpeg image from memory.
+//----------------------------------------------------------------------------
+// $Id$
+
+//---
+// Original code from Thomas G. Lane, header comment follows.
+//---
+
+/*
+ * memsrc.c
+ *
+ * Copyright (C) 1994-1996, Thomas G. Lane.
+ * This file is part of the Independent JPEG Group's software.
+ * For conditions of distribution and use, see the accompanying README file.
+ *
+ * This file contains decompression data source routines for the case of
+ * reading JPEG data from a memory buffer that is preloaded with the entire
+ * JPEG file.  This would not seem especially useful at first sight, but
+ * a number of people have asked for it.
+ * This is really just a stripped-down version of jdatasrc.c.  Comparison
+ * of this code with jdatasrc.c may be helpful in seeing how to make
+ * custom source managers for other purposes.
+ */
+
+/* this is not a core library module, so it doesn't define JPEG_INTERNALS */
+/* #include <jinclude.h> */
+
+#include <ossim/imaging/ossimJpegMemSrc.h>
+#include <itkjpeg/8/jerror.h>
+extern "C"
+{
+void ossimJpegErrorExit (j_common_ptr cinfo)
+{
+   /* cinfo->err really points to a my_error_mgr struct, so coerce pointer */
+   ossimJpegErrorPtr myerr = (ossimJpegErrorPtr) cinfo->err;
+   
+   /* Always display the message. */
+   /* We could postpone this until after returning, if we chose. */
+   (*cinfo->err->output_message) (cinfo);
+   
+   /* Return control to the setjmp point */
+   longjmp(myerr->setjmp_buffer, 1);
+}
+
+
+/* Expanded data source object for memory input */
+
+struct ossimJpegSourceMgr
+{
+   struct jpeg_source_mgr pub;	/* public fields */
+   JOCTET eoi_buffer[2];	/* a place to put a dummy EOI */
+};
+
+typedef ossimJpegSourceMgr* ossimJpegSourceMgrPtr;
+
+
+/*
+ * Initialize source --- called by jpeg_read_header
+ * before any data is actually read.
+ */
+
+void init_source (j_decompress_ptr cinfo)
+{
+  /* No work, since jpeg_memory_src set up the buffer pointer and count.
+   * Indeed, if we want to read multiple JPEG images from one buffer,
+   * this *must* not do anything to the pointer.
+   */
+}
+
+
+/*
+ * Fill the input buffer --- called whenever buffer is emptied.
+ *
+ * In this application, this routine should never be called; if it is called,
+ * the decompressor has overrun the end of the input buffer, implying we
+ * supplied an incomplete or corrupt JPEG datastream.  A simple error exit
+ * might be the most appropriate response.
+ *
+ * But what we choose to do in this code is to supply dummy EOI markers
+ * in order to force the decompressor to finish processing and supply
+ * some sort of output image, no matter how corrupted.
+ */
+boolean fill_input_buffer (j_decompress_ptr cinfo)
+{
+  ossimJpegSourceMgrPtr src = (ossimJpegSourceMgrPtr) cinfo->src;
+
+  WARNMS(cinfo, JWRN_JPEG_EOF);
+
+  /* Create a fake EOI marker */
+  src->eoi_buffer[0] = (JOCTET) 0xFF;
+  src->eoi_buffer[1] = (JOCTET) JPEG_EOI;
+  src->pub.next_input_byte = src->eoi_buffer;
+  src->pub.bytes_in_buffer = 2;
+
+  return TRUE;
+}
+
+
+/*
+ * Skip data --- used to skip over a potentially large amount of
+ * uninteresting data (such as an APPn marker).
+ *
+ * If we overrun the end of the buffer, we let fill_input_buffer deal with
+ * it.  An extremely large skip could cause some time-wasting here, but
+ * it really isn't supposed to happen ... and the decompressor will never
+ * skip more than 64K anyway.
+ */
+void skip_input_data (j_decompress_ptr cinfo, long num_bytes)
+{
+  ossimJpegSourceMgrPtr src = (ossimJpegSourceMgrPtr) cinfo->src;
+
+  if (num_bytes > 0) {
+    while (num_bytes > (long) src->pub.bytes_in_buffer) {
+      num_bytes -= (long) src->pub.bytes_in_buffer;
+      (void) fill_input_buffer(cinfo);
+      /* note we assume that fill_input_buffer will never return FALSE,
+       * so suspension need not be handled.
+       */
+    }
+    src->pub.next_input_byte += (size_t) num_bytes;
+    src->pub.bytes_in_buffer -= (size_t) num_bytes;
+  }
+}
+
+
+/*
+ * An additional method that can be provided by data source modules is the
+ * resync_to_restart method for error recovery in the presence of RST markers.
+ * For the moment, this source module just uses the default resync method
+ * provided by the JPEG library.  That method assumes that no backtracking
+ * is possible.
+ */
+
+
+/*
+ * Terminate source --- called by jpeg_finish_decompress
+ * after all data has been read.  Often a no-op.
+ *
+ * NB: *not* called by jpeg_abort or jpeg_destroy; surrounding
+ * application must deal with any cleanup that should happen even
+ * for error exit.
+ */
+void term_source (j_decompress_ptr cinfo)
+{
+  /* no work necessary here */
+}
+
+
+/*
+ * Prepare for input from a memory buffer.
+ */
+void ossimJpegMemorySrc (j_decompress_ptr cinfo,
+                         const JOCTET * buffer,
+                         std::size_t bufsize)
+{
+  ossimJpegSourceMgrPtr src;
+
+  /* The source object is made permanent so that a series of JPEG images
+   * can be read from a single buffer by calling jpeg_memory_src
+   * only before the first one.
+   * This makes it unsafe to use this manager and a different source
+   * manager serially with the same JPEG object.  Caveat programmer.
+   */
+  if (cinfo->src == NULL) {	/* first time for this JPEG object? */
+    cinfo->src = (struct jpeg_source_mgr *)
+      (*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_PERMANENT,
+				  sizeof(ossimJpegSourceMgr));
+  }
+
+  src = (ossimJpegSourceMgrPtr) cinfo->src;
+  src->pub.init_source = init_source;
+  src->pub.fill_input_buffer = fill_input_buffer;
+  src->pub.skip_input_data = skip_input_data;
+  src->pub.resync_to_restart = jpeg_resync_to_restart; /* use default method */
+  src->pub.term_source = term_source;
+
+  src->pub.next_input_byte = buffer;
+  src->pub.bytes_in_buffer = bufsize;
+}
+}
+