<commit_before>/********************************************************************************
    LibVideoGfx - video processing library
    Copyright (C) 2002  Dirk Farin

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 ********************************************************************************/

#include "libvideogfx/graphics/fileio/ppm.hh"
#include "libvideogfx/error.hh"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

namespace videogfx {
  using namespace std;

  // --- low-level PPM writers

  void WritePPM5(const Bitmap<Pixel>& pm, ostream& ostr)
  {
    const int w = pm.AskWidth();
    const int h = pm.AskHeight();

    ostr << "P5\n" << w << ' ' << h << "\n255\n";

    const Pixel*const* Y = pm.AskFrame();

    for (int y=0;y<h;y++)
      ostr.write((char*)Y[y],w);
  }

  void WritePPM6(const Bitmap<Pixel>& r, const Bitmap<Pixel>& g, const Bitmap<Pixel>& b, ostream& ostr)
  {
    const int w = r.AskWidth();
    const int h = r.AskHeight();

    Assert(w==g.AskWidth() && h==g.AskHeight());
    Assert(w==b.AskWidth() && h==b.AskHeight());

    ostr << "P6\n" << w << ' ' << h << "\n255\n";

    uint8* linebuf = new uint8[w*3];

    const Pixel*const* R = r.AskFrame();
    const Pixel*const* G = g.AskFrame();
    const Pixel*const* B = b.AskFrame();

    for (int y=0;y<h;y++)
      {
	uint8* p = linebuf;

	for (int x=0;x<w;x++)
	  {
	    *p++ = R[y][x];
	    *p++ = G[y][x];
	    *p++ = B[y][x];
	  }

	ostr.write((char*)linebuf,w*3);
      }

    delete[] linebuf;
  }

  // --- high-level PPM-writer class

  FileWriter_PPM::FileWriter_PPM()
    : d_yuv_as_grey(false),
      d_rgb_as_grey(false),
      d_grey_as_rgb(false)
  {
  }

  void FileWriter_PPM::Write(const Image<Pixel>& img,const char* filename)
  {
    ofstream ostr;
    ostr.open(filename, std::ios::out | std::ios::binary);
    Write(img,ostr);
  }

  void FileWriter_PPM::Write(const Image<Pixel>& img,ostream& ostr)
  {
    ImageParam param = img.AskParam();

    if (param.colorspace == Colorspace_RGB)
      {
	if (d_rgb_as_grey)
	  WritePPM5(img.AskBitmapG(),ostr);
	else
	  WritePPM6(img.AskBitmapR(),img.AskBitmapG(),img.AskBitmapB(),ostr);
      }
    else if (param.colorspace == Colorspace_Greyscale)
      {
	if (d_grey_as_rgb)
	  WritePPM6(img.AskBitmapY(),img.AskBitmapY(),img.AskBitmapY(),ostr);
	else
	  WritePPM5(img.AskBitmapY(),ostr);
      }
    else if (param.colorspace == Colorspace_YUV)
      {
	if (d_yuv_as_grey)
	  WritePPM5(img.AskBitmapY(),ostr);
	else
	  {
	    AssertDescr(false,"cannot save YUV as PPM in RGB colorspace");
	  }
      }
    else
      {
	AssertDescr(false,"cannot save this colorspace as PPM file");
      }
  }

  // --- convenience function for PPM writing

  void WriteImage_PPM(ostream& stream, const Image<Pixel>& img)
  {
    FileWriter_PPM writer;
    writer.Write(img,stream);
  }

  void WriteImage_PPM(const char* filename, const Image<Pixel>& img)
  {
    FileWriter_PPM writer;
    writer.WriteYUVAsGreyscale(true);
    writer.Write(img,filename);
  }

  // --- PPM reading functions

  static bool is_whiteline(const char* b)
  {
    for (int i=0; b[i]; i++)
      if (!isspace(b[i]))
	return false;

    return true;
  }

  void ReadImage_PPM(Image<Pixel>& dest,const char* filename)
  {
    ifstream istr(filename);
    ReadImage_PPM(dest,istr);
  }

  void ReadImage_PPM(Image<Pixel>& dest,istream& stream)
  {
    char buffer[100+1];
    stream.getline(buffer,100);

    if (strlen(buffer)!=2 || buffer[0]!='P')
      { throw Excpt_Text(ErrSev_Error,"input is not a PPM format file"); }

    bool greyscale;
    if (buffer[1]=='5')
      greyscale=true;
    else if (buffer[1]=='6')
      greyscale=false;
    else
      { throw Excpt_Text(ErrSev_Error,"input is not a type 5 or type 6 PPM format file"); }

    int width,height,maxval;

    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    sscanf(buffer,"%d %d",&width,&height);
    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    maxval=atoi(buffer);

    if (maxval > 255)
      { throw Excpt_Text(ErrSev_Error,"cannot read PPM file with maximum pixel-value > 255"); }

    ImageParam param = dest.AskParam();
    param.width  = width;
    param.height = height;

    if (greyscale)
      {
	param.colorspace = Colorspace_Greyscale;
	dest.Create(param);

	Pixel*const* Y = dest.AskFrameY();

	for (int y=0;y<height;y++)
	  stream.read((char*)Y[y],width);
      }
    else
      {
	param.colorspace = Colorspace_RGB;
	dest.Create(param);

	uint8* linebuf = new uint8[width * 3];

	Pixel*const* r = dest.AskFrameR();
	Pixel*const* g = dest.AskFrameG();
	Pixel*const* b = dest.AskFrameB();

	for (int y=0;y<height;y++)
	  {
	    stream.read((char*)linebuf,width*3);

	    uint8* p = linebuf;
	    uint8* rp = r[y];
	    uint8* gp = g[y];
	    uint8* bp = b[y];

	    for (int x=0;x<width;x++)
	      {
		*rp++ = *p++;
		*gp++ = *p++;
		*bp++ = *p++;
	      }
	  }

	delete[] linebuf;
      }
  }

  static void swapbytes(uint16* linebuf, int n_shorts)
  {
    uint8* p8 = (uint8*)linebuf;
    for (int i=0;i<n_shorts;i++)
      {
	swap(p8[0],p8[1]);
	p8+=2;
      }
  }

  int ReadImage_PPM(Image<uint16>& dest,std::istream& stream)
  {
    char buffer[100+1];
    stream.getline(buffer,100);

    if (strlen(buffer)!=2 || buffer[0]!='P')
      { throw Excpt_Text(ErrSev_Error,"input is not a PPM format file"); }

    bool greyscale;
    if (buffer[1]=='5')
      greyscale=true;
    else if (buffer[1]=='6')
      greyscale=false;
    else
      { throw Excpt_Text(ErrSev_Error,"input is not a type 5 or type 6 PPM format file"); }

    int width,height,maxval;

    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    sscanf(buffer,"%d %d",&width,&height);
    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    maxval=atoi(buffer);

    if (maxval > 65535)
      { throw Excpt_Text(ErrSev_Error,"cannot read PPM file with maximum pixel-value > 65535"); }

    int nbytes_per_pixel=1;
    if (maxval>255) nbytes_per_pixel=2;

    ImageParam param = dest.AskParam();
    param.width  = width;
    param.height = height;

    if (nbytes_per_pixel==2)
      {
	if (greyscale)
	  {
	    param.colorspace = Colorspace_Greyscale;
	    dest.Create(param);

	    uint16*const* Y = dest.AskFrameY();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)Y[y],width*2);
#if !WORDS_BIGENDIAN
		swapbytes(Y[y], width);
#endif
	      }
	  }
	else
	  {
	    param.colorspace = Colorspace_RGB;
	    dest.Create(param);

	    uint16* linebuf = new uint16[width * 3];

	    uint16*const* r = dest.AskFrameR();
	    uint16*const* g = dest.AskFrameG();
	    uint16*const* b = dest.AskFrameB();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)linebuf,width*2*3);
#if !WORDS_BIGENDIAN
		swapbytes(linebuf, width*3);
#endif

		uint16* p = linebuf;
		uint16* rp = r[y];
		uint16* gp = g[y];
		uint16* bp = b[y];

		for (int x=0;x<width;x++)
		  {
		    *rp++ = *p++;
		    *gp++ = *p++;
		    *bp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
      }
    else // nbytes_per_pixel==1
      {
	if (greyscale)
	  {
	    param.colorspace = Colorspace_Greyscale;
	    dest.Create(param);

	    uint16*const* Y = dest.AskFrameY();

	    uint8* linebuf = new uint8[width];

	    for (int y=0;y<height;y++)
	      {
		uint8* p = linebuf;
		uint16* yp = Y[y];

		stream.read((char*)linebuf,width);

		for (int x=0;x<width;x++)
		  {
		    *yp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
	else
	  {
	    param.colorspace = Colorspace_RGB;
	    dest.Create(param);

	    uint8* linebuf = new uint8[width * 3];

	    uint16*const* r = dest.AskFrameR();
	    uint16*const* g = dest.AskFrameG();
	    uint16*const* b = dest.AskFrameB();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)linebuf,width*3);

		uint8* p = linebuf;
		uint16* rp = r[y];
		uint16* gp = g[y];
		uint16* bp = b[y];

		for (int x=0;x<width;x++)
		  {
		    *rp++ = *p++;
		    *gp++ = *p++;
		    *bp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
      }

    return maxval;
  }

  int ReadImage_PPM(Image<uint16>& dest,const char* filename)
  {
    ifstream istr(filename);
    return ReadImage_PPM(dest,istr);
  }

  void WriteImage_PPM(std::ostream& ostr, const Image<uint16>& img, int maxVal)
  {
    ImageParam param = img.AskParam();

    bool isColor = (param.colorspace == Colorspace_RGB);
    ostr << "P" << (isColor ? "6" : "5") << "\n"
	 << param.width << " " << param.height << "\n"
	 << maxVal << "\n";

    uint16* linebuf = NULL;
    int w = img.getWidth();
    int h = img.getHeight();

    if (isColor)
      {
	linebuf = new uint16[w*3];

	for (int y=0;y<h;y++)
	  {
	    uint16* l = linebuf;
	    for (int x=0;x<w;x++)
	      {
		*l++ = img.AskFrameR()[y][x];
		*l++ = img.AskFrameG()[y][x];
		*l++ = img.AskFrameB()[y][x];
	      }

#if !WORDS_BIGENDIAN
	    swapbytes(linebuf, 3*w);
#endif

	    ostr.write((const char*)linebuf, 2*3*w);
	  }
      }
    else
      {
	linebuf = new uint16[w];

	for (int y=0;y<h;y++)
	  {
	    memcpy(linebuf, img.AskFrameY()[y], w*2);
#if !WORDS_BIGENDIAN
	    swapbytes(linebuf, w);
#endif

	    ostr.write((const char*)linebuf, 2*w);
	  }
      }

    delete[] linebuf;
  }

  void WriteImage_PPM(const char* filename, const Image<uint16>& img, int maxVal)
  {
    ofstream ostr(filename);
    return WriteImage_PPM(ostr, img, maxVal);
  }
}
<commit_msg>add reading of more PPM formats and write 16bit images in binary mode<commit_after>/********************************************************************************
    LibVideoGfx - video processing library
    Copyright (C) 2002  Dirk Farin

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 ********************************************************************************/

#include "libvideogfx/graphics/fileio/ppm.hh"
#include "libvideogfx/error.hh"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

namespace videogfx {
  using namespace std;

  // --- low-level PPM writers

  void WritePPM5(const Bitmap<Pixel>& pm, ostream& ostr)
  {
    const int w = pm.AskWidth();
    const int h = pm.AskHeight();

    ostr << "P5\n" << w << ' ' << h << "\n255\n";

    const Pixel*const* Y = pm.AskFrame();

    for (int y=0;y<h;y++)
      ostr.write((char*)Y[y],w);
  }

  void WritePPM6(const Bitmap<Pixel>& r, const Bitmap<Pixel>& g, const Bitmap<Pixel>& b, ostream& ostr)
  {
    const int w = r.AskWidth();
    const int h = r.AskHeight();

    Assert(w==g.AskWidth() && h==g.AskHeight());
    Assert(w==b.AskWidth() && h==b.AskHeight());

    ostr << "P6\n" << w << ' ' << h << "\n255\n";

    uint8* linebuf = new uint8[w*3];

    const Pixel*const* R = r.AskFrame();
    const Pixel*const* G = g.AskFrame();
    const Pixel*const* B = b.AskFrame();

    for (int y=0;y<h;y++)
      {
	uint8* p = linebuf;

	for (int x=0;x<w;x++)
	  {
	    *p++ = R[y][x];
	    *p++ = G[y][x];
	    *p++ = B[y][x];
	  }

	ostr.write((char*)linebuf,w*3);
      }

    delete[] linebuf;
  }

  // --- high-level PPM-writer class

  FileWriter_PPM::FileWriter_PPM()
    : d_yuv_as_grey(false),
      d_rgb_as_grey(false),
      d_grey_as_rgb(false)
  {
  }

  void FileWriter_PPM::Write(const Image<Pixel>& img,const char* filename)
  {
    ofstream ostr;
    ostr.open(filename, std::ios::out | std::ios::binary);
    Write(img,ostr);
  }

  void FileWriter_PPM::Write(const Image<Pixel>& img,ostream& ostr)
  {
    ImageParam param = img.AskParam();

    if (param.colorspace == Colorspace_RGB)
      {
	if (d_rgb_as_grey)
	  WritePPM5(img.AskBitmapG(),ostr);
	else
	  WritePPM6(img.AskBitmapR(),img.AskBitmapG(),img.AskBitmapB(),ostr);
      }
    else if (param.colorspace == Colorspace_Greyscale)
      {
	if (d_grey_as_rgb)
	  WritePPM6(img.AskBitmapY(),img.AskBitmapY(),img.AskBitmapY(),ostr);
	else
	  WritePPM5(img.AskBitmapY(),ostr);
      }
    else if (param.colorspace == Colorspace_YUV)
      {
	if (d_yuv_as_grey)
	  WritePPM5(img.AskBitmapY(),ostr);
	else
	  {
	    AssertDescr(false,"cannot save YUV as PPM in RGB colorspace");
	  }
      }
    else
      {
	AssertDescr(false,"cannot save this colorspace as PPM file");
      }
  }

  // --- convenience function for PPM writing

  void WriteImage_PPM(ostream& stream, const Image<Pixel>& img)
  {
    FileWriter_PPM writer;
    writer.Write(img,stream);
  }

  void WriteImage_PPM(const char* filename, const Image<Pixel>& img)
  {
    FileWriter_PPM writer;
    writer.WriteYUVAsGreyscale(true);
    writer.Write(img,filename);
  }

  // --- PPM reading functions

  static bool is_whiteline(const char* b)
  {
    for (int i=0; b[i]; i++)
      if (!isspace(b[i]))
	return false;

    return true;
  }

  void ReadImage_PPM(Image<Pixel>& dest,const char* filename)
  {
    ifstream istr(filename);
    ReadImage_PPM(dest,istr);
  }

  void ReadImage_PPM(Image<Pixel>& dest,istream& stream)
  {
    char buffer[100+1];
    stream.getline(buffer,100);

    if (strlen(buffer)!=2 || buffer[0]!='P')
      { throw Excpt_Text(ErrSev_Error,"input is not a PPM format file"); }

    bool greyscale;
    if (buffer[1]=='5')
      greyscale=true;
    else if (buffer[1]=='6')
      greyscale=false;
    else
      { throw Excpt_Text(ErrSev_Error,"input is not a type 5 or type 6 PPM format file"); }

    int width,height,maxval;

    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    sscanf(buffer,"%d %d",&width,&height);
    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    maxval=atoi(buffer);

    if (maxval > 255)
      { throw Excpt_Text(ErrSev_Error,"cannot read PPM file with maximum pixel-value > 255"); }

    ImageParam param = dest.AskParam();
    param.width  = width;
    param.height = height;

    if (greyscale)
      {
	param.colorspace = Colorspace_Greyscale;
	dest.Create(param);

	Pixel*const* Y = dest.AskFrameY();

	for (int y=0;y<height;y++)
	  stream.read((char*)Y[y],width);
      }
    else
      {
	param.colorspace = Colorspace_RGB;
	dest.Create(param);

	uint8* linebuf = new uint8[width * 3];

	Pixel*const* r = dest.AskFrameR();
	Pixel*const* g = dest.AskFrameG();
	Pixel*const* b = dest.AskFrameB();

	for (int y=0;y<height;y++)
	  {
	    stream.read((char*)linebuf,width*3);

	    uint8* p = linebuf;
	    uint8* rp = r[y];
	    uint8* gp = g[y];
	    uint8* bp = b[y];

	    for (int x=0;x<width;x++)
	      {
		*rp++ = *p++;
		*gp++ = *p++;
		*bp++ = *p++;
	      }
	  }

	delete[] linebuf;
      }
  }

  static void swapbytes(uint16* linebuf, int n_shorts)
  {
    uint8* p8 = (uint8*)linebuf;
    for (int i=0;i<n_shorts;i++)
      {
	swap(p8[0],p8[1]);
	p8+=2;
      }
  }

  int ReadImage_PPM(Image<uint16>& dest,std::istream& stream)
  {
    char buffer[100+1];
    stream.getline(buffer,100);

    if (strlen(buffer)!=2 || buffer[0]!='P')
      { throw Excpt_Text(ErrSev_Error,"input is not a PPM format file"); }

    bool greyscale;
    bool ascii=false;
    if (buffer[1]=='5')
      { greyscale=true; ascii=false; }
    else if (buffer[1]=='6')
      { greyscale=false; ascii=false; }
    else if (buffer[1]=='2')
      { greyscale=true; ascii=true; }
    else if (buffer[1]=='3')
      { greyscale=false; ascii=true; }
    else
      { throw Excpt_Text(ErrSev_Error,"input is not a type 2,3,5 or 6 PPM format file"); }

    int width,height,maxval;

    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    sscanf(buffer,"%d %d",&width,&height);
    do
      {
	stream.getline(buffer,100);
      } while(buffer[0] == '#' || is_whiteline(buffer));
    maxval=atoi(buffer);

    if (maxval > 65535)
      { throw Excpt_Text(ErrSev_Error,"cannot read PPM file with maximum pixel-value > 65535"); }

    int nbytes_per_pixel=1;
    if (maxval>255) nbytes_per_pixel=2;

    ImageParam param = dest.AskParam();
    param.width  = width;
    param.height = height;

    if (ascii)
      {
	if (greyscale)
	  {
	    param.colorspace = Colorspace_Greyscale;
	    dest.Create(param);

	    uint16*const* Y = dest.AskFrameY();

	    for (int y=0;y<height;y++)
	      for (int x=0;x<width;x++)
		{
		  stream >> Y[y][x];
		}
	  }
	else
	  {
	    param.colorspace = Colorspace_RGB;
	    dest.Create(param);

	    uint16*const* r = dest.AskFrameR();
	    uint16*const* g = dest.AskFrameG();
	    uint16*const* b = dest.AskFrameB();

	    for (int y=0;y<height;y++)
	      for (int x=0;x<width;x++)
		{
		  stream >> r[y][x] >> g[y][x] >> b[y][x];
		}
	  }
      }
    else if (nbytes_per_pixel==2)
      {
	if (greyscale)
	  {
	    param.colorspace = Colorspace_Greyscale;
	    dest.Create(param);

	    uint16*const* Y = dest.AskFrameY();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)Y[y],width*2);
#if !WORDS_BIGENDIAN
		swapbytes(Y[y], width);
#endif
	      }
	  }
	else
	  {
	    param.colorspace = Colorspace_RGB;
	    dest.Create(param);

	    uint16* linebuf = new uint16[width * 3];

	    uint16*const* r = dest.AskFrameR();
	    uint16*const* g = dest.AskFrameG();
	    uint16*const* b = dest.AskFrameB();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)linebuf,width*2*3);
#if !WORDS_BIGENDIAN
		swapbytes(linebuf, width*3);
#endif

		uint16* p = linebuf;
		uint16* rp = r[y];
		uint16* gp = g[y];
		uint16* bp = b[y];

		for (int x=0;x<width;x++)
		  {
		    *rp++ = *p++;
		    *gp++ = *p++;
		    *bp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
      }
    else // nbytes_per_pixel==1
      {
	if (greyscale)
	  {
	    param.colorspace = Colorspace_Greyscale;
	    dest.Create(param);

	    uint16*const* Y = dest.AskFrameY();

	    uint8* linebuf = new uint8[width];

	    for (int y=0;y<height;y++)
	      {
		uint8* p = linebuf;
		uint16* yp = Y[y];

		stream.read((char*)linebuf,width);

		for (int x=0;x<width;x++)
		  {
		    *yp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
	else
	  {
	    param.colorspace = Colorspace_RGB;
	    dest.Create(param);

	    uint8* linebuf = new uint8[width * 3];

	    uint16*const* r = dest.AskFrameR();
	    uint16*const* g = dest.AskFrameG();
	    uint16*const* b = dest.AskFrameB();

	    for (int y=0;y<height;y++)
	      {
		stream.read((char*)linebuf,width*3);

		uint8* p = linebuf;
		uint16* rp = r[y];
		uint16* gp = g[y];
		uint16* bp = b[y];

		for (int x=0;x<width;x++)
		  {
		    *rp++ = *p++;
		    *gp++ = *p++;
		    *bp++ = *p++;
		  }
	      }

	    delete[] linebuf;
	  }
      }

    return maxval;
  }

  int ReadImage_PPM(Image<uint16>& dest,const char* filename)
  {
    ifstream istr(filename);
    return ReadImage_PPM(dest,istr);
  }

  void WriteImage_PPM(std::ostream& ostr, const Image<uint16>& img, int maxVal)
  {
    ImageParam param = img.AskParam();

    bool isColor = (param.colorspace == Colorspace_RGB);
    ostr << "P" << (isColor ? "6" : "5") << "\n"
	 << param.width << " " << param.height << "\n"
	 << maxVal << "\n";

    uint16* linebuf = NULL;
    int w = img.getWidth();
    int h = img.getHeight();

    if (isColor)
      {
	linebuf = new uint16[w*3];

	for (int y=0;y<h;y++)
	  {
	    uint16* l = linebuf;
	    for (int x=0;x<w;x++)
	      {
		*l++ = img.AskFrameR()[y][x];
		*l++ = img.AskFrameG()[y][x];
		*l++ = img.AskFrameB()[y][x];
	      }

#if !WORDS_BIGENDIAN
	    swapbytes(linebuf, 3*w);
#endif

	    ostr.write((const char*)linebuf, 2*3*w);
	  }
      }
    else
      {
	linebuf = new uint16[w];

	for (int y=0;y<h;y++)
	  {
	    memcpy(linebuf, img.AskFrameY()[y], w*2);
#if !WORDS_BIGENDIAN
	    swapbytes(linebuf, w);
#endif

	    ostr.write((const char*)linebuf, 2*w);
	  }
      }

    delete[] linebuf;
  }

  void WriteImage_PPM(const char* filename, const Image<uint16>& img, int maxVal)
  {
    ofstream ostr;
    ostr.open(filename, std::ios::out | std::ios::binary);
    return WriteImage_PPM(ostr, img, maxVal);
  }
}
<|endoftext|>


<commit_before>// Copyright (c) 2013 Intel Corporation. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "xwalk/application/browser/application_system_linux.h"

#include "base/command_line.h"
#include "dbus/bus.h"
#include "xwalk/application/browser/application_service_provider_linux.h"
#include "xwalk/dbus/dbus_manager.h"
#include "xwalk/runtime/browser/xwalk_runner.h"

namespace xwalk {
namespace application {

ApplicationSystemLinux::ApplicationSystemLinux(RuntimeContext* runtime_context)
    : ApplicationSystem(runtime_context) {
  CommandLine* cmd_line = CommandLine::ForCurrentProcess();
  if (XWalkRunner::GetInstance()->is_running_as_service()) {
    service_provider_.reset(
        new ApplicationServiceProviderLinux(application_service(),
                                            application_storage(),
                                            dbus_manager().session_bus()));
  }
}

ApplicationSystemLinux::~ApplicationSystemLinux() {}

DBusManager& ApplicationSystemLinux::dbus_manager() {
  if (!dbus_manager_)
    dbus_manager_.reset(new DBusManager());
  return *dbus_manager_.get();
}

}  // namespace application
}  // namespace xwalk
<commit_msg>[Application][Linux] Remove unused CommandLine<commit_after>// Copyright (c) 2013 Intel Corporation. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "xwalk/application/browser/application_system_linux.h"

#include "dbus/bus.h"
#include "xwalk/application/browser/application_service_provider_linux.h"
#include "xwalk/dbus/dbus_manager.h"
#include "xwalk/runtime/browser/xwalk_runner.h"

namespace xwalk {
namespace application {

ApplicationSystemLinux::ApplicationSystemLinux(RuntimeContext* runtime_context)
    : ApplicationSystem(runtime_context) {
  if (XWalkRunner::GetInstance()->is_running_as_service()) {
    service_provider_.reset(
        new ApplicationServiceProviderLinux(application_service(),
                                            application_storage(),
                                            dbus_manager().session_bus()));
  }
}

ApplicationSystemLinux::~ApplicationSystemLinux() {}

DBusManager& ApplicationSystemLinux::dbus_manager() {
  if (!dbus_manager_)
    dbus_manager_.reset(new DBusManager());
  return *dbus_manager_.get();
}

}  // namespace application
}  // namespace xwalk
<|endoftext|>


<commit_before>/*
  ExtendoHand.cpp
  Created by Joshua Shinavier, 2012-2014
  Released into the public domain.
*/

#include "ExtendoHand.h"

////////////////////////////////////////////////////////////////////////////////

#define MOTION_X_PIN A0
#define MOTION_Y_PIN A1
#define MOTION_Z_PIN A2

#define SPEAKER_PIN  9  // PWM preferred
#define RGB_LED_PIN  13
#define VIBRO_PIN    2

////////////////////////////////////////////////////////////////////////////////

// allows OSC to dispatch messages to non-member functions which call member functions
ExtendoHand *instance;

ExtendoHand::ExtendoHand(): osc(EXO_HAND) {
    instance = this;
    lastHeartbeat = 0;
    loopTimeHandler = NULL;
    nineAxis = true;
    setContext("default");
}

void ExtendoHand::setLoopTimeHandler(void (*handler)(double)) {
    loopTimeHandler = handler;
}

void ExtendoHand::enableInput(boolean b) {
    inputEnabled = b;
}

void ExtendoHand::useNineAxisSensors(boolean b) {
    nineAxis = b;
}

////////////////////////////////////////////////////////////////////////////////

ExtendOSC *ExtendoHand::getOSC() {
    return &osc;
}

double ExtendoHand::getLoopTime() {
    return loopTime;
}

////////////////////////////////////////////////////////////////////////////////

// three-axis

#include <MMA7361.h>

MMA7361 motionSensor(MOTION_X_PIN, MOTION_Y_PIN, MOTION_Z_PIN);

// nine-axis

#include <Wire.h>
#include <I2Cdev.h>
#include <ADXL345.h>
#include <ITG3200.h>
#include <HMC5883L.h>

ADXL345 accel;
ITG3200 gyro;
HMC5883L magnet;

////////////////////////////////////////////////////////////////////////////////

#include <Droidspeak.h>

Droidspeak droidspeak(SPEAKER_PIN);

////////////////////////////////////////////////////////////////////////////////

#include <Morse.h>

int morseStopTest() {
    // no need to abort
    return 0;
}

void error(const char *message) {
    instance->getOSC()->sendError(message);
}

Morse morse(SPEAKER_PIN, morseStopTest, error);

////////////////////////////////////////////////////////////////////////////////

#include <RGBLED.h>
#include <Adafruit_NeoPixel.h>

Adafruit_NeoPixel leds = Adafruit_NeoPixel(1, RGB_LED_PIN, NEO_GRB + NEO_KHZ800);

void ExtendoHand::setColor(unsigned long color) {
    leds.setPixelColor(0, (uint32_t) color);
    leds.show();
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::vibrate(unsigned long durationMs) {
    digitalWrite(VIBRO_PIN, HIGH);
    delay(durationMs);
    digitalWrite(VIBRO_PIN, LOW);
}

void ExtendoHand::playTone(unsigned int frequency, unsigned long durationMs) {
    tone(SPEAKER_PIN, frequency, durationMs);
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::setup() {
    pinMode(VIBRO_PIN, OUTPUT);

    leds.begin();
    setColor(RGB_YELLOW);
    droidspeak.speakPowerUpPhrase();

    osc.beginSerial();

    bundleIn = new OSCBundle();

    if (nineAxis) {
        // adjust the power settings after you call this method if you want the accelerometer
        // to enter standby mode, or another less demanding mode of operation
        accel.setRange(1); // 4g
        accel.setFullResolution(1); // maintain 4mg/LSB scale factor (irrespective of range)
        accel.initialize();
        if (!accel.testConnection()) {
            osc.sendError("ADXL345 connection failed");
        } else {
            randomSeed(accel.getAccelerationX() - accel.getAccelerationY() + accel.getAccelerationZ());
        }

        gyro.initialize();
        if (!gyro.testConnection()) {
            osc.sendError("ITG3200 connection failed");
        }

        magnet.initialize();
        if (!magnet.testConnection()) {
            osc.sendError("HMC5883L connection failed");
        }
    } else {
        // TODO: random seed using 9-axis sensor
        randomSeed(motionSensor.rawX() + motionSensor.rawY() + motionSensor.rawZ());

        // 1.5g constants, sampled 2014-06-21
        motionSensor.calibrateX(272, 794);
        motionSensor.calibrateY(332, 841);
        motionSensor.calibrateZ(175, 700);
    }

    // delay the serial open phrase until the random number generator has been seeded
    setColor(RGB_GREEN);
    droidspeak.speakSerialOpenPhrase();

    setColor(RGB_BLACK);

    vibrate(500);
    osc.sendInfo("Extend-o-Hand is ready");

    loopCount = 0;
    loopThreshold = 100;
    // initial value for loop time gets us through the first half second or so
    loopTime = 4.0/1000;
    loopStartTime = millis();
}

////////////////////////////////////////////////////////////////////////////////

const char* ExtendoHand::getContext() {
    return contextName;
}

void ExtendoHand::setContext(const char *context) {
    strcpy(contextName, context);
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::sendPingReply() {
    OSCMessage m(EXO_HAND_PING_REPLY);
    m.add((uint64_t) micros());

    osc.sendOSC(m);
}

#ifdef HEARTBEAT_MS
void ExtendoHand::sendHeartbeatMessage(unsigned long now) {
    OSCMessage m(EXO_HAND_HEARTBEAT);
    m.add((uint64_t) now);
    osc.sendOSC(m);
}
#endif

////////////////////////////////////////////////////////////////////////////////
// non-member OSC handler functions

void handleAudioToneMessage(class OSCMessage &m) {
  /*
    if (!osc.validArgs(m, 2)) return;

    int32_t frequency = m.getInt(0);
    int32_t duration = m.getInt(1);

    if (frequency <= 0 || frequency > 20000) {
        osc.sendError("frequency out of range: %d", (int) frequency);
    } else if (duration <= 0) {
        osc.sendError("duration must be a positive number");
    } else if (duration > 60000) {
        osc.sendError("duration too long");
    } else {
        tone(SPEAKER_PIN, (int) frequency);
        delay((unsigned long) duration);
        noTone(SPEAKER_PIN);
    }
    */
}

void handleContextSetMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    char buffer[32];
    m.getString(0, buffer, m.getDataLength(0) + 1);
    instance->setContext(buffer);
}

/*
void ExtendoHand::handleMorseMessage(class OSCMessage &m) {
    if (!osc.validArgs(m, 1)) return;

    int length = m.getDataLength(0);
    char buffer[length+1];
    m.getString(0, buffer, length+1);

    morse.playMorseString(buffer);
}
*/

void handlePingMessage(class OSCMessage &m) {
    instance->sendPingReply();
}

void handleRGBSetMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    int32_t color = m.getInt(0);

    if (color < 0 || color > 0xffffff) {
        instance->getOSC()->sendError("color out of range: %d", (long) color);
    } else {
        instance->setColor(color);
    }
}

void handleVibroMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    int32_t d = m.getInt(0);

    if (d <= 0) {
        instance->getOSC()->sendError("duration must be a positive number");
    } else if (d > 60000) {
        instance->getOSC()->sendError("duration too long");
    } else {
        instance->vibrate((unsigned long) d);
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::handleOSCBundle(class OSCBundle &bundle) {
    if (bundle.hasError()) {
        osc.sendOSCBundleError(bundle);
    } else if (!(0
        || bundle.dispatch(EXO_HAND_AUDIO_TONE, handleAudioToneMessage)
        || bundle.dispatch(EXO_HAND_CONTEXT_SET, handleContextSetMessage)
        //|| bundle.dispatch(EXO_HAND_MORSE, handleMorseMessage)
        || bundle.dispatch(EXO_HAND_PING, handlePingMessage)
        || bundle.dispatch(EXO_HAND_RGB_SET, handleRGBSetMessage)
        || bundle.dispatch(EXO_HAND_VIBRO, handleVibroMessage)
        )) {
        osc.sendError("no messages dispatched");
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::getAcceleration(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t ax, ay, az;
        accel.getAcceleration(&ax, &ay, &az);

        // approximate g values, per calibration with a specific sensor
        *x = ax / 230.0 - 0.05;
        *y = ay / 230.0;
        *z = az / 230.0;
        //*x = ax/230.0/16;
        //*y = ay/230.0/16;
        //*z = az/230.0/16;
    } else {
        *x = motionSensor.accelX();
        *y = motionSensor.accelY();
        *z = motionSensor.accelZ();
    }
}

void ExtendoHand::getRotation(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t gx, gy, gz;
        gyro.getRotation(&gx, &gy, &gz);
        *x = gx;
        *y = gy;
        *z = gz;
    } else {
          *x = 0;
          *y = 0;
          *z = 0;
    }
}

void ExtendoHand::getHeading(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t mx, my, mz;
        magnet.getHeading(&mx, &my, &mz);
        *x = mx;
        *y = my;
        *z = mz;
    } else {
        *x = 0;
        *y = 0;
        *z = 0;
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::updateSamplingRate() {
    if (nineAxis) {
        // the sensor's sampling rate should exceed Extend-o-Hand's loop rate
        uint8_t sampleRate = loopTime <= 0.00125
            ? 0xe  // 1600 Hz
            : loopTime <= 0.0025
            ? 0xd  // 800 Hz
            : 0xc; // 400 Hz

        // uncomment only for development
        //osc.sendInfo("setting sensor sampling rate to %d based on loop time of %d micros",
        //    sampleRate, (int)(loopTime*1000000));

        // adjust the power settings after you call these methods if you want the sensors
        // to enter standby mode, or another less demanding mode of operation
        accel.setRate(sampleRate);
        gyro.setRate(sampleRate);
        magnet.setDataRate(sampleRate);
    }
}

unsigned long ExtendoHand::beginLoop() {
    // no need for micros() here, and it's simpler to avoid overflow
    unsigned long now = millis();

    if (inputEnabled) {
        if (osc.receiveOSCBundle(*bundleIn)) {
            handleOSCBundle(*bundleIn);
            bundleIn->empty();
            delete bundleIn;
            bundleIn = new OSCBundle();
        }
    }

// TODO: never applies
#ifdef HEARTBEAT_MS
    if (now - lastHeartbeat > HEARTBEAT_MS) {
        sendHeartbeatMessage(now);
        lastHeartbeat = now;
    }
#endif // HEARTBEAT_MS

    // periodically adjust the loop time used in band-pass filtering (among potentially other applications)
    if (++loopCount >= loopThreshold) {
        loopTime = (now - loopStartTime)/1000.0/loopThreshold;

        updateSamplingRate();

        if (loopTimeHandler) {
            loopTimeHandler(loopTime);
        }

        loopCount = 0;
        loopStartTime = now;
    }

    return now;
}
<commit_msg>Beginning to align Extend-o-Hand and Typeatron OSC<commit_after>/*
  ExtendoHand.cpp
  Created by Joshua Shinavier, 2012-2014
  Released into the public domain.
*/

#include "ExtendoHand.h"

////////////////////////////////////////////////////////////////////////////////

#define MOTION_X_PIN A0
#define MOTION_Y_PIN A1
#define MOTION_Z_PIN A2

#define SPEAKER_PIN  9  // PWM preferred
#define RGB_LED_PIN  13
#define VIBRO_PIN    2

////////////////////////////////////////////////////////////////////////////////

// allows OSC to dispatch messages to non-member functions which call member functions
ExtendoHand *instance;

ExtendoHand::ExtendoHand(): osc(EXO_HAND) {
    instance = this;
    lastHeartbeat = 0;
    loopTimeHandler = NULL;
    nineAxis = true;
    setContext("default");
}

void ExtendoHand::setLoopTimeHandler(void (*handler)(double)) {
    loopTimeHandler = handler;
}

void ExtendoHand::enableInput(boolean b) {
    inputEnabled = b;
}

void ExtendoHand::useNineAxisSensors(boolean b) {
    nineAxis = b;
}

////////////////////////////////////////////////////////////////////////////////

ExtendOSC *ExtendoHand::getOSC() {
    return &osc;
}

double ExtendoHand::getLoopTime() {
    return loopTime;
}

////////////////////////////////////////////////////////////////////////////////

// three-axis

#include <MMA7361.h>

MMA7361 motionSensor(MOTION_X_PIN, MOTION_Y_PIN, MOTION_Z_PIN);

// nine-axis

#include <Wire.h>
#include <I2Cdev.h>
#include <ADXL345.h>
#include <ITG3200.h>
#include <HMC5883L.h>

ADXL345 accel;
ITG3200 gyro;
HMC5883L magnet;

////////////////////////////////////////////////////////////////////////////////

#include <Droidspeak.h>

Droidspeak droidspeak(SPEAKER_PIN);

////////////////////////////////////////////////////////////////////////////////

#include <Morse.h>

int morseStopTest() {
    // no need to abort
    return 0;
}

void error(const char *message) {
    instance->getOSC()->sendError(message);
}

Morse morse(SPEAKER_PIN, morseStopTest, error);

////////////////////////////////////////////////////////////////////////////////

#include <RGBLED.h>
#include <Adafruit_NeoPixel.h>

Adafruit_NeoPixel leds = Adafruit_NeoPixel(1, RGB_LED_PIN, NEO_GRB + NEO_KHZ800);

void ExtendoHand::setColor(unsigned long color) {
    leds.setPixelColor(0, (uint32_t) color);
    leds.show();
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::vibrate(unsigned long durationMs) {
    digitalWrite(VIBRO_PIN, HIGH);
    delay(durationMs);
    digitalWrite(VIBRO_PIN, LOW);
}

void ExtendoHand::playTone(unsigned int frequency, unsigned long durationMs) {
    tone(SPEAKER_PIN, frequency, durationMs);
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::setup() {
    pinMode(VIBRO_PIN, OUTPUT);

    leds.begin();
    setColor(RGB_YELLOW);
    droidspeak.speakPowerUpPhrase();

    osc.beginSerial();

    bundleIn = new OSCBundle();

    if (nineAxis) {
        // adjust the power settings after you call this method if you want the accelerometer
        // to enter standby mode, or another less demanding mode of operation
        accel.setRange(1); // 4g
        accel.setFullResolution(1); // maintain 4mg/LSB scale factor (irrespective of range)
        accel.initialize();
        if (!accel.testConnection()) {
            osc.sendError("ADXL345 connection failed");
        } else {
            randomSeed(accel.getAccelerationX() - accel.getAccelerationY() + accel.getAccelerationZ());
        }

        gyro.initialize();
        if (!gyro.testConnection()) {
            osc.sendError("ITG3200 connection failed");
        }

        magnet.initialize();
        if (!magnet.testConnection()) {
            osc.sendError("HMC5883L connection failed");
        }
    } else {
        // TODO: random seed using 9-axis sensor
        randomSeed(motionSensor.rawX() + motionSensor.rawY() + motionSensor.rawZ());

        // 1.5g constants, sampled 2014-06-21
        motionSensor.calibrateX(272, 794);
        motionSensor.calibrateY(332, 841);
        motionSensor.calibrateZ(175, 700);
    }

    // delay the serial open phrase until the random number generator has been seeded
    setColor(RGB_GREEN);
    droidspeak.speakSerialOpenPhrase();

    setColor(RGB_BLACK);

    vibrate(500);
    osc.sendInfo("Extend-o-Hand is ready");

    loopCount = 0;
    loopThreshold = 100;
    // initial value for loop time gets us through the first half second or so
    loopTime = 4.0/1000;
    loopStartTime = millis();
}

////////////////////////////////////////////////////////////////////////////////

const char* ExtendoHand::getContext() {
    return contextName;
}

void ExtendoHand::setContext(const char *context) {
    strcpy(contextName, context);
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::sendPingReply() {
    OSCMessage m(EXO_HAND_PING_REPLY);
    m.add((uint64_t) micros());

    osc.sendOSC(m);
}

#ifdef HEARTBEAT_MS
void ExtendoHand::sendHeartbeatMessage(unsigned long now) {
    OSCMessage m(EXO_HAND_HEARTBEAT);
    m.add((uint64_t) now);
    osc.sendOSC(m);
}
#endif

////////////////////////////////////////////////////////////////////////////////
// non-member OSC handler functions

void handleAudioToneMessage(class OSCMessage &m) {
    if (!osc.validArgs(m, 2)) return;

    int32_t frequency = m.getInt(0);
    int32_t duration = m.getInt(1);

    if (frequency <= 0 || frequency > 20000) {
        osc.sendError("frequency out of range: %d", (int) frequency);
    } else if (duration <= 0) {
        osc.sendError("duration must be a positive number");
    } else if (duration > 60000) {
        osc.sendError("duration too long");
    } else {
        tone(SPEAKER_PIN, (int) frequency);
        delay((unsigned long) duration);
        noTone(SPEAKER_PIN);
    }
}

void handleContextSetMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    char buffer[32];
    m.getString(0, buffer, m.getDataLength(0) + 1);
    instance->setContext(buffer);
}

/*
void ExtendoHand::handleMorseMessage(class OSCMessage &m) {
    if (!osc.validArgs(m, 1)) return;

    int length = m.getDataLength(0);
    char buffer[length+1];
    m.getString(0, buffer, length+1);

    morse.playMorseString(buffer);
}
*/

void handlePingMessage(class OSCMessage &m) {
    instance->sendPingReply();
}

void handleRGBSetMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    int32_t color = m.getInt(0);

    if (color < 0 || color > 0xffffff) {
        instance->getOSC()->sendError("color out of range: %d", (long) color);
    } else {
        instance->setColor(color);
    }
}

void handleVibroMessage(class OSCMessage &m) {
    if (!instance->getOSC()->validArgs(m, 1)) return;

    int32_t d = m.getInt(0);

    if (d <= 0) {
        instance->getOSC()->sendError("duration must be a positive number");
    } else if (d > 60000) {
        instance->getOSC()->sendError("duration too long");
    } else {
        instance->vibrate((unsigned long) d);
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::handleOSCBundle(class OSCBundle &bundle) {
    if (bundle.hasError()) {
        osc.sendOSCBundleError(bundle);
    } else if (!(0
        || bundle.dispatch(EXO_HAND_AUDIO_TONE, handleAudioToneMessage)
        || bundle.dispatch(EXO_HAND_CONTEXT_SET, handleContextSetMessage)
        //|| bundle.dispatch(EXO_HAND_MORSE, handleMorseMessage)
        || bundle.dispatch(EXO_HAND_PING, handlePingMessage)
        || bundle.dispatch(EXO_HAND_RGB_SET, handleRGBSetMessage)
        || bundle.dispatch(EXO_HAND_VIBRO, handleVibroMessage)
        )) {
        osc.sendError("no messages dispatched");
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::getAcceleration(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t ax, ay, az;
        accel.getAcceleration(&ax, &ay, &az);

        // approximate g values, per calibration with a specific sensor
        *x = ax / 230.0 - 0.05;
        *y = ay / 230.0;
        *z = az / 230.0;
        //*x = ax/230.0/16;
        //*y = ay/230.0/16;
        //*z = az/230.0/16;
    } else {
        *x = motionSensor.accelX();
        *y = motionSensor.accelY();
        *z = motionSensor.accelZ();
    }
}

void ExtendoHand::getRotation(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t gx, gy, gz;
        gyro.getRotation(&gx, &gy, &gz);
        *x = gx;
        *y = gy;
        *z = gz;
    } else {
          *x = 0;
          *y = 0;
          *z = 0;
    }
}

void ExtendoHand::getHeading(double *x, double *y, double *z) {
    if (nineAxis) {
        int16_t mx, my, mz;
        magnet.getHeading(&mx, &my, &mz);
        *x = mx;
        *y = my;
        *z = mz;
    } else {
        *x = 0;
        *y = 0;
        *z = 0;
    }
}

////////////////////////////////////////////////////////////////////////////////

void ExtendoHand::updateSamplingRate() {
    if (nineAxis) {
        // the sensor's sampling rate should exceed Extend-o-Hand's loop rate
        uint8_t sampleRate = loopTime <= 0.00125
            ? 0xe  // 1600 Hz
            : loopTime <= 0.0025
            ? 0xd  // 800 Hz
            : 0xc; // 400 Hz

        // uncomment only for development
        //osc.sendInfo("setting sensor sampling rate to %d based on loop time of %d micros",
        //    sampleRate, (int)(loopTime*1000000));

        // adjust the power settings after you call these methods if you want the sensors
        // to enter standby mode, or another less demanding mode of operation
        accel.setRate(sampleRate);
        gyro.setRate(sampleRate);
        magnet.setDataRate(sampleRate);
    }
}

unsigned long ExtendoHand::beginLoop() {
    // no need for micros() here, and it's simpler to avoid overflow
    unsigned long now = millis();

    if (inputEnabled) {
        if (osc.receiveOSCBundle(*bundleIn)) {
            handleOSCBundle(*bundleIn);
            bundleIn->empty();
            delete bundleIn;
            bundleIn = new OSCBundle();
        }
    }

// TODO: never applies
#ifdef HEARTBEAT_MS
    if (now - lastHeartbeat > HEARTBEAT_MS) {
        sendHeartbeatMessage(now);
        lastHeartbeat = now;
    }
#endif // HEARTBEAT_MS

    // periodically adjust the loop time used in band-pass filtering (among potentially other applications)
    if (++loopCount >= loopThreshold) {
        loopTime = (now - loopStartTime)/1000.0/loopThreshold;

        updateSamplingRate();

        if (loopTimeHandler) {
            loopTimeHandler(loopTime);
        }

        loopCount = 0;
        loopStartTime = now;
    }

    return now;
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestContext.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkSmartPointer.h"
#include "vtkObjectFactory.h"
#include "vtkContext2D.h"
#include "vtkTransform2D.h"
#include "vtkContextItem.h"
#include "vtkContextView.h"
#include "vtkContextScene.h"
#include "vtkPen.h"
#include "vtkBrush.h"
#include "vtkTextProperty.h"
#include "vtkOpenGLContextDevice2D.h"
#include "vtkPoints2D.h"

#include "vtkRegressionTestImage.h"

#define VTK_CREATE(type, name) \
  vtkSmartPointer<type> name = vtkSmartPointer<type>::New()

//----------------------------------------------------------------------------
class ContextTest : public vtkContextItem
{
public:
  static ContextTest *New();
  vtkTypeMacro(ContextTest, vtkContextItem);
  // Paint event for the chart, called whenever the chart needs to be drawn
  virtual bool Paint(vtkContext2D *painter);
};

//----------------------------------------------------------------------------
int TestContext( int argc, char * argv [] )
{
  // Set up a 2D context view, context test object and add it to the scene
  VTK_CREATE(vtkContextView, view);
  view->GetRenderer()->SetBackground(1.0, 1.0, 1.0);
  view->GetRenderWindow()->SetSize(800, 600);
  VTK_CREATE(ContextTest, test);
  view->GetScene()->AddItem(test);

  // Force the use of the freetype based rendering strategy
  vtkOpenGLContextDevice2D::SafeDownCast(view->GetContext()->GetDevice())
      ->SetStringRendererToFreeType();

  view->GetRenderWindow()->SetMultiSamples(0);
  view->GetRenderWindow()->Render();

  int retVal = vtkRegressionTestImage(view->GetRenderWindow());
  return !retVal;
}

// Make our new derived class to draw a diagram
vtkStandardNewMacro(ContextTest);
// This function aims to test the primitives provided by the 2D API.
bool ContextTest::Paint(vtkContext2D *painter)
{
  // Test the string drawing functionality of the context
  painter->GetTextProp()->SetVerticalJustificationToCentered();
  painter->GetTextProp()->SetJustificationToCentered();
  painter->GetTextProp()->SetColor(0.0, 0.0, 0.0);
  painter->GetTextProp()->SetFontSize(24);
  painter->GetTextProp()->SetFontFamilyToArial();
  painter->GetPen()->SetColor(0, 0, 0, 255);
  painter->GetBrush()->SetColor(0, 0, 0, 255);
  painter->DrawString(400, 25, "OpenGL is used as a backend to the context.");

  // Draw some individual lines of different thicknesses.
  for (int i = 0; i < 10; ++i)
    {
    painter->GetPen()->SetColor(255,
                                static_cast<unsigned char>(float(i)*25.0),
                                0);
    painter->GetPen()->SetWidth(1.0 + float(i));
    painter->DrawLine(10, 50 + float(i)*10, 60, 50 + float(i)*10);
    }

  // Use the draw lines function now to draw a shape.
  VTK_CREATE(vtkPoints2D, points);
  points->SetNumberOfPoints(30);
  for (int i = 0; i < 30; ++i)
    {
    double point[2] = { float(i) * 25.0 + 10.0,
                        sin(float(i) / 5.0) * 100.0 + 200.0 };
    points->SetPoint(i, point);
    }
  painter->GetPen()->SetColor(0, 255, 0);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoly(points);

  // Now to draw some points
  painter->GetPen()->SetColor(0, 0, 255);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoint(10, 10);
  painter->DrawPoint(790, 10);
  painter->DrawPoint(10, 590);
  painter->DrawPoint(790, 590);

  // Draw some individual lines of different thicknesses.
  for (int i = 0; i < 10; ++i)
    {
    painter->GetPen()->SetColor(0,
                                static_cast<unsigned char>(float(i)*25.0),
                                255);
    painter->GetPen()->SetWidth(1.0 + float(i));
    painter->DrawPoint(75, 50 + float(i)*10);
    }

  painter->GetPen()->SetColor(0, 0, 255);
  painter->GetPen()->SetWidth(3.0);
  painter->DrawPoints(points);

  // Now draw a rectangle
  painter->GetPen()->SetColor(100, 200, 255);
  painter->GetPen()->SetWidth(3.0);
  painter->GetBrush()->SetColor(100, 255, 100);
  painter->DrawRect(100, 50, 200, 100);

  // Add in an arbitrary quad
  painter->GetPen()->SetColor(159, 0, 255);
  painter->GetPen()->SetWidth(1.0);
  painter->GetBrush()->SetColor(100, 55, 0, 200);
  painter->DrawQuad(350, 50, 375, 150,
                    525, 199, 666, 45);

  // Now to test out the transform...
  VTK_CREATE(vtkTransform2D, transform);
  transform->Translate(20, 200);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(255, 0, 0);
  painter->GetPen()->SetWidth(6.0);
  painter->DrawPoly(points);

  transform->Translate(0, 10);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(0, 0, 200);
  painter->GetPen()->SetWidth(2.0);
  painter->DrawPoints(points);

  transform->Translate(0, -20);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(100, 0, 200);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoints(points);

  // Now for an ellipse...
  painter->GetPen()->SetColor(0, 0, 0);
  painter->GetPen()->SetWidth(1.0);
  painter->GetBrush()->SetColor(0, 0, 100, 69);
  painter->DrawEllipse(110.0, 89.0, 20, 100);

  return true;
}
<commit_msg>STYLE: Got rid of VTK_CREATE macro, added interactor logic.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestContext.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkSmartPointer.h"
#include "vtkObjectFactory.h"
#include "vtkContext2D.h"
#include "vtkTransform2D.h"
#include "vtkContextItem.h"
#include "vtkContextView.h"
#include "vtkContextScene.h"
#include "vtkPen.h"
#include "vtkBrush.h"
#include "vtkTextProperty.h"
#include "vtkOpenGLContextDevice2D.h"
#include "vtkPoints2D.h"

#include "vtkRegressionTestImage.h"

//----------------------------------------------------------------------------
class ContextTest : public vtkContextItem
{
public:
  static ContextTest *New();
  vtkTypeMacro(ContextTest, vtkContextItem);
  // Paint event for the chart, called whenever the chart needs to be drawn
  virtual bool Paint(vtkContext2D *painter);
};

//----------------------------------------------------------------------------
int TestContext( int argc, char * argv [] )
{
  // Set up a 2D context view, context test object and add it to the scene
  vtkSmartPointer<vtkContextView> view = vtkSmartPointer<vtkContextView>::New();
  view->GetRenderer()->SetBackground(1.0, 1.0, 1.0);
  view->GetRenderWindow()->SetSize(800, 600);
  vtkSmartPointer<ContextTest> test = vtkSmartPointer<ContextTest>::New();
  view->GetScene()->AddItem(test);

  // Force the use of the freetype based rendering strategy
  vtkOpenGLContextDevice2D::SafeDownCast(view->GetContext()->GetDevice())
      ->SetStringRendererToFreeType();

  view->GetRenderWindow()->SetMultiSamples(0);
  view->GetRenderWindow()->Render();

  int retVal = vtkRegressionTestImage(view->GetRenderWindow());
  if(retVal == vtkRegressionTester::DO_INTERACTOR)
    {
    view->GetInteractor()->Initialize();
    view->GetInteractor()->Start();
    }
  return !retVal;
}

// Make our new derived class to draw a diagram
vtkStandardNewMacro(ContextTest);
// This function aims to test the primitives provided by the 2D API.
bool ContextTest::Paint(vtkContext2D *painter)
{
  // Test the string drawing functionality of the context
  painter->GetTextProp()->SetVerticalJustificationToCentered();
  painter->GetTextProp()->SetJustificationToCentered();
  painter->GetTextProp()->SetColor(0.0, 0.0, 0.0);
  painter->GetTextProp()->SetFontSize(24);
  painter->GetTextProp()->SetFontFamilyToArial();
  painter->GetPen()->SetColor(0, 0, 0, 255);
  painter->GetBrush()->SetColor(0, 0, 0, 255);
  painter->DrawString(400, 25, "OpenGL is used as a backend to the context.");

  // Draw some individual lines of different thicknesses.
  for (int i = 0; i < 10; ++i)
    {
    painter->GetPen()->SetColor(255,
                                static_cast<unsigned char>(float(i)*25.0),
                                0);
    painter->GetPen()->SetWidth(1.0 + float(i));
    painter->DrawLine(10, 50 + float(i)*10, 60, 50 + float(i)*10);
    }

  // Use the draw lines function now to draw a shape.
  vtkSmartPointer<vtkPoints2D> points = vtkSmartPointer<vtkPoints2D>::New();
  points->SetNumberOfPoints(30);
  for (int i = 0; i < 30; ++i)
    {
    double point[2] = { float(i) * 25.0 + 10.0,
                        sin(float(i) / 5.0) * 100.0 + 200.0 };
    points->SetPoint(i, point);
    }
  painter->GetPen()->SetColor(0, 255, 0);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoly(points);

  // Now to draw some points
  painter->GetPen()->SetColor(0, 0, 255);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoint(10, 10);
  painter->DrawPoint(790, 10);
  painter->DrawPoint(10, 590);
  painter->DrawPoint(790, 590);

  // Draw some individual lines of different thicknesses.
  for (int i = 0; i < 10; ++i)
    {
    painter->GetPen()->SetColor(0,
                                static_cast<unsigned char>(float(i)*25.0),
                                255);
    painter->GetPen()->SetWidth(1.0 + float(i));
    painter->DrawPoint(75, 50 + float(i)*10);
    }

  painter->GetPen()->SetColor(0, 0, 255);
  painter->GetPen()->SetWidth(3.0);
  painter->DrawPoints(points);

  // Now draw a rectangle
  painter->GetPen()->SetColor(100, 200, 255);
  painter->GetPen()->SetWidth(3.0);
  painter->GetBrush()->SetColor(100, 255, 100);
  painter->DrawRect(100, 50, 200, 100);

  // Add in an arbitrary quad
  painter->GetPen()->SetColor(159, 0, 255);
  painter->GetPen()->SetWidth(1.0);
  painter->GetBrush()->SetColor(100, 55, 0, 200);
  painter->DrawQuad(350, 50, 375, 150,
                    525, 199, 666, 45);

  // Now to test out the transform...
  vtkSmartPointer<vtkTransform2D> transform = vtkSmartPointer<vtkTransform2D>::New();
  transform->Translate(20, 200);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(255, 0, 0);
  painter->GetPen()->SetWidth(6.0);
  painter->DrawPoly(points);

  transform->Translate(0, 10);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(0, 0, 200);
  painter->GetPen()->SetWidth(2.0);
  painter->DrawPoints(points);

  transform->Translate(0, -20);
  painter->GetDevice()->SetMatrix(transform->GetMatrix());
  painter->GetPen()->SetColor(100, 0, 200);
  painter->GetPen()->SetWidth(5.0);
  painter->DrawPoints(points);

  // Now for an ellipse...
  painter->GetPen()->SetColor(0, 0, 0);
  painter->GetPen()->SetWidth(1.0);
  painter->GetBrush()->SetColor(0, 0, 100, 69);
  painter->DrawEllipse(110.0, 89.0, 20, 100);

  return true;
}
<|endoftext|>


<commit_before>/***************************************************************************\
**
**  tRunTimer.cpp: functions for tRunTimer objects.
**
**  tRunTimer objects are used to keep track of time in a time-evolving
**  simulation model. Their services include keeping track of when it's
**  time to write output, printing the current time to standard output if
**  desired, and writing the current time to a file every so often.
**
**  Version 1.0, Greg Tucker, November 1997
**
**  Potential additions/improvements:
**  - add functions to set output interval and time status notification
**    interval
**
**  $Id: tRunTimer.cpp,v 1.3 1998-06-04 21:27:09 gtucker Exp $
\***************************************************************************/

#include <iostream.h>
#include <fstream.h>
#include <assert.h>

#include "../tInputFile/tInputFile.h"

#include "tRunTimer.h"
//****************************************************
// Constructors
//
// - A default constructor
// - A constructor that sets the run duration and
//   output interval (and if desired sets the option
//   to print time steps to stdout; default is 1,
//   see header file)
// - A constructor that reads run duration and
//   output interval from a tInputFile object (and
//   if desired sets the option
//   to print time steps to stdout; default is 1,
//   see header file)
//
// Note that the notifyInterval is always initialized
// at 1000, but of course this could be changed.
//****************************************************

tRunTimer::tRunTimer()
{
	currentTime = 0;
	endTime = 1;
	outputInterval = 1;
	nextOutputTime = 0;
	optPrintEachTime = 1;
	notifyInterval = 1000;
	nextNotify = 0;
}

tRunTimer::tRunTimer( double duration, double opint, int optprint )
{
	currentTime = 0;
	endTime = duration;
	outputInterval = opint;
	nextOutputTime = 0;
	optPrintEachTime = optprint;
	notifyInterval = 1000;
	nextNotify = 0;
}

tRunTimer::tRunTimer( tInputFile &infile, int optprint )
{
	currentTime = 0;
	endTime = infile.ReadItem( endTime, "RUNTIME" );
	outputInterval = infile.ReadItem( outputInterval, "OPINTRVL" );
	nextOutputTime = 0;
	optPrintEachTime = optprint;
	notifyInterval = 1000;
	nextNotify = 0;
}

//****************************************************
// Start
//
// Sets the current time and run duration.
//****************************************************
void tRunTimer::Start( double start, double end )
{
   assert( end > start );
   currentTime = start;
   endTime = end;
}

//****************************************************
// getCurrentTime
//
// Returns the current time.
//****************************************************
double tRunTimer::getCurrentTime()
{
	return currentTime;
}

//****************************************************
// RemainingTime
//
// Returns the remaining time.
//****************************************************
double tRunTimer::RemainingTime()
{
	return endTime - currentTime;
}

//****************************************************
// Advance
//
// Increments the current time by dt.
// Returns 1 if there is still time
// remaining, 0 if the time is up.
//****************************************************
int tRunTimer::Advance( double dt )
{
	currentTime += dt;
	return( currentTime < endTime );
}

//****************************************************
// ReportTimeStatus
//
// Reports the current time to a file and to
// standard output if desired. Output to file is
// only done at selected intervals.
// Compares currentTime with nextNotify to see
// whether it's time to update the time status file,
// and if so opens the file and writes the current
// time (overwriting any previous contents).
// Regardless of the status of nextNotify, if
// optPrintEachTime is selected, the current time is
// reported to cout.
//****************************************************
void tRunTimer::ReportTimeStatus()
{
	if( optPrintEachTime ) cout << currentTime << endl;
	if( currentTime >= nextNotify )
	{
		timeStatusFile.open( "run.time" );
		assert( timeStatusFile.good() );
		timeStatusFile << currentTime << endl;
		timeStatusFile.close();
		nextNotify += notifyInterval;
	}
}

//*************************************************
// CheckOutputTime
//
// Checks to see whether it's time to write output
// yet.
//*************************************************
int tRunTimer::CheckOutputTime()
{
	if( currentTime>=nextOutputTime )
	{
		nextOutputTime += outputInterval;
		return 1;
	}
	else return 0;
}

//*************************************************
// IsFinished
//
// Checks to see whether the run is finished.
//*************************************************
int tRunTimer::IsFinished()
{
	return( currentTime >= endTime );
}

<commit_msg>Nicole changed the constructor using the input file so that if layering is read in, the start time is set to the time that the layers are read from.<commit_after>/***************************************************************************\
**
**  tRunTimer.cpp: functions for tRunTimer objects.
**
**  tRunTimer objects are used to keep track of time in a time-evolving
**  simulation model. Their services include keeping track of when it's
**  time to write output, printing the current time to standard output if
**  desired, and writing the current time to a file every so often.
**
**  Version 1.0, Greg Tucker, November 1997
**
**  Potential additions/improvements:
**  - add functions to set output interval and time status notification
**    interval
**
**  $Id: tRunTimer.cpp,v 1.4 1998-08-04 20:46:18 nmgaspar Exp $
\***************************************************************************/

#include <iostream.h>
#include <fstream.h>
#include <assert.h>

#include "../tInputFile/tInputFile.h"

#include "tRunTimer.h"
//****************************************************
// Constructors
//
// - A default constructor
// - A constructor that sets the run duration and
//   output interval (and if desired sets the option
//   to print time steps to stdout; default is 1,
//   see header file)
// - A constructor that reads run duration and
//   output interval from a tInputFile object (and
//   if desired sets the option
//   to print time steps to stdout; default is 1,
//   see header file)
//
// Note that the notifyInterval is always initialized
// at 1000, but of course this could be changed.
//****************************************************

tRunTimer::tRunTimer()
{
	currentTime = 0;
	endTime = 1;
	outputInterval = 1;
	nextOutputTime = 0;
	optPrintEachTime = 1;
	notifyInterval = 1000;
	nextNotify = 0;
}

tRunTimer::tRunTimer( double duration, double opint, int optprint )
{
	currentTime = 0;
	endTime = duration;
	outputInterval = opint;
	nextOutputTime = 0;
	optPrintEachTime = optprint;
	notifyInterval = 1000;
	nextNotify = 0;
}

tRunTimer::tRunTimer( tInputFile &infile, int optprint )
{
	endTime = infile.ReadItem( endTime, "RUNTIME" );
	outputInterval = infile.ReadItem( outputInterval, "OPINTRVL" );
	optPrintEachTime = optprint;
	notifyInterval = 1000;
  double help;
  //If you are reading in layering information, the timer should
  //be set to the time in which the layers were output, since
  //time is tracked in the layers and restarting at time zero
  //would make the layer times non-sensical.
  help = infile.ReadItem( help, "OPTREADLAYER" );
  if(help>0){
     help = infile.ReadItem( help, "INPUTTIME" );
     currentTime = help;
     endTime += help;
     nextOutputTime = help;
     nextNotify = help;
  }
  else{
     nextOutputTime = 0;
     nextNotify = 0;
     currentTime = 0;
  }

}

//****************************************************
// Start
//
// Sets the current time and run duration.
//****************************************************
void tRunTimer::Start( double start, double end )
{
   assert( end > start );
   currentTime = start;
   endTime = end;
}

//****************************************************
// getCurrentTime
//
// Returns the current time.
//****************************************************
double tRunTimer::getCurrentTime()
{
	return currentTime;
}

//****************************************************
// RemainingTime
//
// Returns the remaining time.
//****************************************************
double tRunTimer::RemainingTime()
{
	return endTime - currentTime;
}

//****************************************************
// Advance
//
// Increments the current time by dt.
// Returns 1 if there is still time
// remaining, 0 if the time is up.
//****************************************************
int tRunTimer::Advance( double dt )
{
	currentTime += dt;
	return( currentTime < endTime );
}

//****************************************************
// ReportTimeStatus
//
// Reports the current time to a file and to
// standard output if desired. Output to file is
// only done at selected intervals.
// Compares currentTime with nextNotify to see
// whether it's time to update the time status file,
// and if so opens the file and writes the current
// time (overwriting any previous contents).
// Regardless of the status of nextNotify, if
// optPrintEachTime is selected, the current time is
// reported to cout.
//****************************************************
void tRunTimer::ReportTimeStatus()
{
	if( optPrintEachTime ) cout << currentTime << endl;
	if( currentTime >= nextNotify )
	{
		timeStatusFile.open( "run.time" );
		assert( timeStatusFile.good() );
		timeStatusFile << currentTime << endl;
		timeStatusFile.close();
		nextNotify += notifyInterval;
	}
}

//*************************************************
// CheckOutputTime
//
// Checks to see whether it's time to write output
// yet.
//*************************************************
int tRunTimer::CheckOutputTime()
{
	if( currentTime>=nextOutputTime )
	{
		nextOutputTime += outputInterval;
		return 1;
	}
	else return 0;
}

//*************************************************
// IsFinished
//
// Checks to see whether the run is finished.
//*************************************************
int tRunTimer::IsFinished()
{
	return( currentTime >= endTime );
}

<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkRegistrator3D2D.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2000 National Library of Medicine
  All rights reserved.

  See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkRegistrator3D2D.h"
#include <vnl/algo/vnl_svd.h>

/*********************************************************************
 *
 *        Creator
 *
 *********************************************************************/
itkRegistrator3D2D::itkRegistrator3D2D()
{

  Tolerance	  = 0.001;
  InTolerance = 1.000;
  Potential   = 0.0;
  potentialRange = 0.001;

  for(unsigned int i=0; i<6; i++)
  {
    Delta[i]     = 0.0;
    Uncertain[i] = 0.0;
  }

  maxNumberOfIterations = 10;

  PlanarError = 0;
  Jacobian    = 0;

}

/*********************************************************************
 *
 *        Destructor
 *
 *********************************************************************/
itkRegistrator3D2D::~itkRegistrator3D2D()
{
  if( Jacobian )
  {
    delete Jacobian;
    Jacobian = 0;
  }

  if( PlanarError )
  {
    delete PlanarError;
    PlanarError = 0;
  }

}

/*********************************************************************
 *
 *    Set the Tolerance in the MeanSquareError to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetTolerance(double tol)
{
	Tolerance = tol;
}

/*********************************************************************
 *
 *    Set the InTolerance in the MeanSquareError to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetInTolerance(double tol)
{
	InTolerance = tol;
}

/*********************************************************************
 *
 *    Set Potential Range
 *
 *********************************************************************/
void itkRegistrator3D2D::SetPotentialRange(double range)
{
	potentialRange = range;
}

/*********************************************************************
 *
 *    Set the maximum number of iterations before to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetMaximumNumberOfIterations(unsigned int num)
{
  maxNumberOfIterations = num;
}

/*********************************************************************
 *
 *    Set Translational Uncertainties
 *
 *********************************************************************/
void itkRegistrator3D2D::SetTranslationUncertainty(
                              const Vector3D & uncertainty)
{
	Uncertain[0] = uncertainty[0];
	Uncertain[1] = uncertainty[1];
	Uncertain[2] = uncertainty[2];
}

/*********************************************************************
 *
 *    Set Rotational Uncertainties
 *
 *********************************************************************/
void itkRegistrator3D2D::SetRotationUncertainty(
                              const Vector3D & uncertainty)
{
	Uncertain[3] = uncertainty[0];
	Uncertain[4] = uncertainty[1];
	Uncertain[5] = uncertainty[2];
}

/*****************************************************
 *
 *      Get the value of the Mean Square Error
 *
 *****************************************************/
double itkRegistrator3D2D::GetMeanSquareError(void) const
{
	return MeanSquareError;
}

/*****************************************************
 *
 *      Get the value of the Potential
 *
 *****************************************************/
double itkRegistrator3D2D::GetPotential(void) const
{
	return Potential;
}

/*****************************************************
 *
 *      Get the value of the Potential
 *
 *****************************************************/
double itkRegistrator3D2D::GetPotentialRange(void) const
{
	return potentialRange;
}

/*****************************************************
 *
 *        Stop the registration process
 *
 *****************************************************/
void itkRegistrator3D2D::StopRegistration(void)
{
	stopRegistration = true;
}

/*****************************************************
 *
 *    Set the extrinsic transformation
 *
 *****************************************************/
void itkRegistrator3D2D::SetExtrinsicTransform(const Transform3D & matrix)
{

  extrinsicTransform = matrix;
  ResetRegistration();

}

/*****************************************************
 *
 *    Set the intrinsic transformation
 *
 *****************************************************/
void itkRegistrator3D2D::SetIntrinsicTransform(const Transform3D & matrix)
{

  intrinsicTransform = matrix;
  ResetRegistration();

}

/*****************************************************
 *
 *          Compute Jacobian
 *
 *****************************************************/
void itkRegistrator3D2D::ComputeJacobian(void)
{

  Transform3D cameraTotal = intrinsicTransform * extrinsicTransform *
    cumulatedCorrectionTransform;

  double sumSquares = 0.0;

  Potential  = 0.0;
  const double pr2 = potentialRange * potentialRange;

	const unsigned int numPoints = AssociatedPoints.size();

  // this section performs only the minimum set of operations
  // needed to get the 2D projection.  Doing a complete 4x4
  // will use some more operations.
  // Unfortunately, in this way the code depends on the storage
  // order used for the transfrom implementation.
  // Column major order is assumed here.

  const double Xx = cameraTotal(0,0);
  const double Xy = cameraTotal(0,1);
  const double Xz = cameraTotal(0,2);
  const double Xw = cameraTotal(0,3);

  const double Yx = cameraTotal(1,0);
  const double Yy = cameraTotal(1,1);
  const double Yz = cameraTotal(1,2);
  const double Yw = cameraTotal(1,3);

  const double Wx = cameraTotal(3,0);
  const double Wy = cameraTotal(3,1);
  const double Wz = cameraTotal(3,2);
  const double Ww = cameraTotal(3,3);

  vnl_matrix<double> & mJacobian = *Jacobian;

  for(unsigned int i=0; i<AssociatedPoints.size(); i+= 2)
  {

    const Point3D pp = AssociatedPoints[i].GetPoint3D();

    const double x = pp[0];
    const double y = pp[1];
    const double z = pp[2];

    const double Qx = Xx*x +  Xy*y + Xz*z + Xw;
    const double Qy = Yx*x +  Yy*y + Yz*z + Yw;

    const double Qw = Wx*x +  Wy*y + Wz*z + Ww;

    const double Qwi2 = 1.0/(Qw*Qw);

    mJacobian(0, i ) = ( Xx*Qw - Wx*Qx ) * Qwi2;
    mJacobian(0,i+1) = ( Yx*Qw - Wx*Qy ) * Qwi2;

    mJacobian(1, i ) = ( Xy*Qw - Wy*Qx ) * Qwi2;
    mJacobian(1,i+1) = ( Yy*Qw - Wy*Qy ) * Qwi2;

    mJacobian(2, i ) = ( Xz*Qw - Wz*Qx ) * Qwi2;
    mJacobian(2,i+1) = ( Yz*Qw - Wz*Qy ) * Qwi2;

    mJacobian(3, i ) = (Qw*(y*Xz-z*Xy)-Qx*(y*Wz-z*Wy)) * Qwi2;
    mJacobian(3,i+1) = (Qw*(y*Yz-z*Yy)-Qy*(y*Wz-z*Wy)) * Qwi2;

    mJacobian(4, i ) = (Qw*(z*Xx-x*Xz)-Qx*(z*Wx-x*Wz)) * Qwi2;
    mJacobian(4,i+1) = (Qw*(z*Yx-x*Yz)-Qy*(z*Wx-x*Wz)) * Qwi2;

    mJacobian(5, i ) = (Qw*(x*Xy-y*Xx)-Qx*(x*Wy-y*Wx)) * Qwi2;
    mJacobian(5,i+1) = (Qw*(x*Yy-y*Yx)-Qy*(x*Wy-y*Wx)) * Qwi2;

    Point3D win = cameraTotal * pp;
    Point2D qq = AssociatedPoints[i].GetPoint2D();

    const double dx =  qq[0] - win[0];
    const double dy =  qq[1] - win[1];

    PlanarError[i]		=  dx;
    PlanarError[i+1]	=  dy;

    const double squaredDistance = dx*dx + dy*dy;
    sumSquares += squaredDistance;
    Potential  += 1.0/(1.0+(squaredDistance/pr2));

  }

  MeanSquareError = sumSquares  / (double)(numPoints * 2);

}

/*****************************************************
 *
 *          Perform Registration
 *
 *****************************************************/
void itkRegistrator3D2D::PerformRegistration(void)
{

	const double StableConvergenceRate = 1e-7;
  MeanSquareError = InTolerance - Tolerance;

  double oldMeanSquareError = 0.0;

  if( AssociatedPoints.empty() )
  {
    throw itkRegistrator3D2DException("There are no associated points");
  }

	// Initialize vectors;
	{
	for(unsigned int i = 0; i<6; i++) {
		Delta[i] = 0.0;
		}
	}

  numberOfIterations = 0;
	while (  numberOfIterations < maxNumberOfIterations )
  {

		if (MeanSquareError < Tolerance)
    {
      stopRegistration = true;
      break;
    }

		Transform3D translationCorrection;
    // translationCorrection.Translate(Delta[0],Delta[1],Delta[2]);

		Transform3D rotationCorrectionX;
		Transform3D rotationCorrectionY;
		Transform3D rotationCorrectionZ;
    // rotationCorrectionX.RotateX(Delta[3]);
    // rotationCorrectionY.RotateY(Delta[4]);
    // rotationCorrectionZ.RotateZ(Delta[5]);

    cumulatedCorrectionTransform *= translationCorrection;
    cumulatedCorrectionTransform *= rotationCorrectionX;
    cumulatedCorrectionTransform *= rotationCorrectionY;
    cumulatedCorrectionTransform *= rotationCorrectionZ;

    ComputeJacobian();

		if( MeanSquareError < Tolerance ) {
			StopRegistration();
			break;
			}

		if (MeanSquareError > InTolerance)
    {
      stopRegistration = true;
      break;
    }

    const double ConvergenceRate =
		fabs(MeanSquareError-oldMeanSquareError)/MeanSquareError;

    if( ConvergenceRate < StableConvergenceRate )
    {
      StopRegistration();
      break;
    }

		oldMeanSquareError = MeanSquareError;

		// do a least squares solution modification
    try
    {
      LeastSquareSolution();
    }
    catch( exception e )
    {
	    cerr << e.what() << endl;
      break;
    }

    numberOfIterations++;
  }

}

/****************************************************************
 *
 * This method uses a Least Square Algorithm to find "Delta"
 *		from:
 *					Jacobian*Delta = PlanarError
 *
 *		Jacobian is the matrix relating x,y error to variation of
 *		translations and rotations of points in 3D space
 *
 *		PlanarError is the diference between planar points and
 *		the current projection of the associated 3D points
 *
 ****************************************************************/
void itkRegistrator3D2D::LeastSquareSolution(void)
{

	const int dof = 6;
	const int colSize = 2*AssociatedPoints.size();

  if( colSize == 0 ) return;

  vnl_matrix<double> M(dof,dof);

	// compute Jacobian^t * Jacobian
	// this matrix is column ordered
	// in a one dimensional array
   M = (*Jacobian) * Jacobian->transpose();

  // Add Uncertainties
  for(int c=0; c<dof; c++) {
    M[c][c] += Uncertain[c];
    }

  vnl_vector<double> JP = (*Jacobian) * (*PlanarError);

  Delta = vnl_svd<double>(M).solve(JP);

}

/*****************************************************
 *
 *      Load associated points
 *
 *****************************************************/
void itkRegistrator3D2D::LoadAssociatedPoints(const vector<PairPoint3D2D> & externalList)
{
	AssociatedPoints.clear();
  AssociatedPoints.assign(externalList.begin(),externalList.end());
  Allocate();
}

/*****************************************************
 *
 *      Reset Registration
 *
 *****************************************************/
void itkRegistrator3D2D::ResetRegistration(void)
{
  cumulatedCorrectionTransform.set_identity();
}

/*****************************************************
 *
 *      Return the Transformation
 *
 *****************************************************/
const itkRegistrator3D2D::Transform3D & itkRegistrator3D2D::GetTransformation( void ) const
{
  return cumulatedCorrectionTransform;
}

/*****************************************************
 *
 *      Return the real number of iterations
 *
 *****************************************************/
unsigned int itkRegistrator3D2D::GetNumberOfIterationsPerformed( void ) const
{
  return numberOfIterations;
}

/*****************************************************
 *
 *      Allocate memory for internal arrays
 *
 *****************************************************/
void itkRegistrator3D2D::Allocate(void)
{

	const unsigned int numPoints = AssociatedPoints.size();
  const unsigned int column    = numPoints * 2;

  if( Jacobian )
  {
    delete Jacobian;
  }

  try
  {
    Jacobian = new vnl_matrix<double>(6,column);
  }
  catch(...)
  {
		throw itkRegistrator3D2DException("Registration Iterations: memory allocation for Jacobian");
  }

  if( PlanarError )
  {
    delete PlanarError;
  }

  try
  {
    PlanarError = new vnl_vector<double>(column);
  }
  catch(...)
  {
		throw itkRegistrator3D2DException("Registration Iterations: memory allocation for PlanarError");
  }

}

<commit_msg>ENH: The computation of Jacobian^T * Jacobian is improved by using vnl_fastops::AtA()<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkRegistrator3D2D.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2000 National Library of Medicine
  All rights reserved.

  See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkRegistrator3D2D.h"
#include <vnl/algo/vnl_svd.h>
#include <vnl/vnl_fastops.h>

/*********************************************************************
 *
 *        Creator
 *
 *********************************************************************/
itkRegistrator3D2D::itkRegistrator3D2D()
{

  Tolerance	  = 0.001;
  InTolerance = 1.000;
  Potential   = 0.0;
  potentialRange = 0.001;

  for(unsigned int i=0; i<6; i++)
  {
    Delta[i]     = 0.0;
    Uncertain[i] = 0.0;
  }

  maxNumberOfIterations = 10;

  PlanarError = 0;
  Jacobian    = 0;

}

/*********************************************************************
 *
 *        Destructor
 *
 *********************************************************************/
itkRegistrator3D2D::~itkRegistrator3D2D()
{
  if( Jacobian )
  {
    delete Jacobian;
    Jacobian = 0;
  }

  if( PlanarError )
  {
    delete PlanarError;
    PlanarError = 0;
  }

}

/*********************************************************************
 *
 *    Set the Tolerance in the MeanSquareError to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetTolerance(double tol)
{
	Tolerance = tol;
}

/*********************************************************************
 *
 *    Set the InTolerance in the MeanSquareError to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetInTolerance(double tol)
{
	InTolerance = tol;
}

/*********************************************************************
 *
 *    Set Potential Range
 *
 *********************************************************************/
void itkRegistrator3D2D::SetPotentialRange(double range)
{
	potentialRange = range;
}

/*********************************************************************
 *
 *    Set the maximum number of iterations before to stop registration
 *
 *********************************************************************/
void itkRegistrator3D2D::SetMaximumNumberOfIterations(unsigned int num)
{
  maxNumberOfIterations = num;
}

/*********************************************************************
 *
 *    Set Translational Uncertainties
 *
 *********************************************************************/
void itkRegistrator3D2D::SetTranslationUncertainty(
                              const Vector3D & uncertainty)
{
	Uncertain[0] = uncertainty[0];
	Uncertain[1] = uncertainty[1];
	Uncertain[2] = uncertainty[2];
}

/*********************************************************************
 *
 *    Set Rotational Uncertainties
 *
 *********************************************************************/
void itkRegistrator3D2D::SetRotationUncertainty(
                              const Vector3D & uncertainty)
{
	Uncertain[3] = uncertainty[0];
	Uncertain[4] = uncertainty[1];
	Uncertain[5] = uncertainty[2];
}

/*****************************************************
 *
 *      Get the value of the Mean Square Error
 *
 *****************************************************/
double itkRegistrator3D2D::GetMeanSquareError(void) const
{
	return MeanSquareError;
}

/*****************************************************
 *
 *      Get the value of the Potential
 *
 *****************************************************/
double itkRegistrator3D2D::GetPotential(void) const
{
	return Potential;
}

/*****************************************************
 *
 *      Get the value of the Potential
 *
 *****************************************************/
double itkRegistrator3D2D::GetPotentialRange(void) const
{
	return potentialRange;
}

/*****************************************************
 *
 *        Stop the registration process
 *
 *****************************************************/
void itkRegistrator3D2D::StopRegistration(void)
{
	stopRegistration = true;
}

/*****************************************************
 *
 *    Set the extrinsic transformation
 *
 *****************************************************/
void itkRegistrator3D2D::SetExtrinsicTransform(const Transform3D & matrix)
{

  extrinsicTransform = matrix;
  ResetRegistration();

}

/*****************************************************
 *
 *    Set the intrinsic transformation
 *
 *****************************************************/
void itkRegistrator3D2D::SetIntrinsicTransform(const Transform3D & matrix)
{

  intrinsicTransform = matrix;
  ResetRegistration();

}

/*****************************************************
 *
 *          Compute Jacobian
 *
 *****************************************************/
void itkRegistrator3D2D::ComputeJacobian(void)
{

  Transform3D cameraTotal = intrinsicTransform * extrinsicTransform *
    cumulatedCorrectionTransform;

  double sumSquares = 0.0;

  Potential  = 0.0;
  const double pr2 = potentialRange * potentialRange;

	const unsigned int numPoints = AssociatedPoints.size();

  // this section performs only the minimum set of operations
  // needed to get the 2D projection.  Doing a complete 4x4
  // will use some more operations.
  // Unfortunately, in this way the code depends on the storage
  // order used for the transfrom implementation.
  // Column major order is assumed here.

  const double Xx = cameraTotal(0,0);
  const double Xy = cameraTotal(0,1);
  const double Xz = cameraTotal(0,2);
  const double Xw = cameraTotal(0,3);

  const double Yx = cameraTotal(1,0);
  const double Yy = cameraTotal(1,1);
  const double Yz = cameraTotal(1,2);
  const double Yw = cameraTotal(1,3);

  const double Wx = cameraTotal(3,0);
  const double Wy = cameraTotal(3,1);
  const double Wz = cameraTotal(3,2);
  const double Ww = cameraTotal(3,3);

  vnl_matrix<double> & mJacobian = *Jacobian;

  for(unsigned int i=0; i<AssociatedPoints.size(); i+= 2)
  {

    const Point3D pp = AssociatedPoints[i].GetPoint3D();

    const double x = pp[0];
    const double y = pp[1];
    const double z = pp[2];

    const double Qx = Xx*x +  Xy*y + Xz*z + Xw;
    const double Qy = Yx*x +  Yy*y + Yz*z + Yw;

    const double Qw = Wx*x +  Wy*y + Wz*z + Ww;

    const double Qwi2 = 1.0/(Qw*Qw);

    mJacobian(0, i ) = ( Xx*Qw - Wx*Qx ) * Qwi2;
    mJacobian(0,i+1) = ( Yx*Qw - Wx*Qy ) * Qwi2;

    mJacobian(1, i ) = ( Xy*Qw - Wy*Qx ) * Qwi2;
    mJacobian(1,i+1) = ( Yy*Qw - Wy*Qy ) * Qwi2;

    mJacobian(2, i ) = ( Xz*Qw - Wz*Qx ) * Qwi2;
    mJacobian(2,i+1) = ( Yz*Qw - Wz*Qy ) * Qwi2;

    mJacobian(3, i ) = (Qw*(y*Xz-z*Xy)-Qx*(y*Wz-z*Wy)) * Qwi2;
    mJacobian(3,i+1) = (Qw*(y*Yz-z*Yy)-Qy*(y*Wz-z*Wy)) * Qwi2;

    mJacobian(4, i ) = (Qw*(z*Xx-x*Xz)-Qx*(z*Wx-x*Wz)) * Qwi2;
    mJacobian(4,i+1) = (Qw*(z*Yx-x*Yz)-Qy*(z*Wx-x*Wz)) * Qwi2;

    mJacobian(5, i ) = (Qw*(x*Xy-y*Xx)-Qx*(x*Wy-y*Wx)) * Qwi2;
    mJacobian(5,i+1) = (Qw*(x*Yy-y*Yx)-Qy*(x*Wy-y*Wx)) * Qwi2;

    Point3D win = cameraTotal * pp;
    Point2D qq = AssociatedPoints[i].GetPoint2D();

    const double dx =  qq[0] - win[0];
    const double dy =  qq[1] - win[1];

    PlanarError[i]		=  dx;
    PlanarError[i+1]	=  dy;

    const double squaredDistance = dx*dx + dy*dy;
    sumSquares += squaredDistance;
    Potential  += 1.0/(1.0+(squaredDistance/pr2));

  }

  MeanSquareError = sumSquares  / (double)(numPoints * 2);

}

/*****************************************************
 *
 *          Perform Registration
 *
 *****************************************************/
void itkRegistrator3D2D::PerformRegistration(void)
{

	const double StableConvergenceRate = 1e-7;
  MeanSquareError = InTolerance - Tolerance;

  double oldMeanSquareError = 0.0;

  if( AssociatedPoints.empty() )
  {
    throw itkRegistrator3D2DException("There are no associated points");
  }

	// Initialize vectors;
	{
	for(unsigned int i = 0; i<6; i++) {
		Delta[i] = 0.0;
		}
	}

  numberOfIterations = 0;
	while (  numberOfIterations < maxNumberOfIterations )
  {

		if (MeanSquareError < Tolerance)
    {
      stopRegistration = true;
      break;
    }

		Transform3D translationCorrection;
    // translationCorrection.Translate(Delta[0],Delta[1],Delta[2]);

		Transform3D rotationCorrectionX;
		Transform3D rotationCorrectionY;
		Transform3D rotationCorrectionZ;
    // rotationCorrectionX.RotateX(Delta[3]);
    // rotationCorrectionY.RotateY(Delta[4]);
    // rotationCorrectionZ.RotateZ(Delta[5]);

    cumulatedCorrectionTransform *= translationCorrection;
    cumulatedCorrectionTransform *= rotationCorrectionX;
    cumulatedCorrectionTransform *= rotationCorrectionY;
    cumulatedCorrectionTransform *= rotationCorrectionZ;

    ComputeJacobian();

		if( MeanSquareError < Tolerance ) {
			StopRegistration();
			break;
			}

		if (MeanSquareError > InTolerance)
    {
      stopRegistration = true;
      break;
    }

    const double ConvergenceRate =
		fabs(MeanSquareError-oldMeanSquareError)/MeanSquareError;

    if( ConvergenceRate < StableConvergenceRate )
    {
      StopRegistration();
      break;
    }

		oldMeanSquareError = MeanSquareError;

		// do a least squares solution modification
    try
    {
      LeastSquareSolution();
    }
    catch( exception e )
    {
	    cerr << e.what() << endl;
      break;
    }

    numberOfIterations++;
  }

}

/****************************************************************
 *
 * This method uses a Least Square Algorithm to find "Delta"
 *		from:
 *					Jacobian*Delta = PlanarError
 *
 *		Jacobian is the matrix relating x,y error to variation of
 *		translations and rotations of points in 3D space
 *
 *		PlanarError is the diference between planar points and
 *		the current projection of the associated 3D points
 *
 ****************************************************************/
void itkRegistrator3D2D::LeastSquareSolution(void)
{

	const int dof = 6;
	const int colSize = 2*AssociatedPoints.size();

  if( colSize == 0 ) return;

  vnl_matrix<double> M(dof,dof);

	// compute Jacobian^t * Jacobian
	// this matrix is column ordered
	// in a one dimensional array
  vnl_fastops::AtA(*Jacobian, &M);

  // Add Uncertainties
  for(int c=0; c<dof; c++) {
    M[c][c] += Uncertain[c];
    }

  vnl_vector<double> JP = (*Jacobian) * (*PlanarError);

  Delta = vnl_svd<double>(M).solve(JP);

}

/*****************************************************
 *
 *      Load associated points
 *
 *****************************************************/
void itkRegistrator3D2D::LoadAssociatedPoints(const vector<PairPoint3D2D> & externalList)
{
	AssociatedPoints.clear();
  AssociatedPoints.assign(externalList.begin(),externalList.end());
  Allocate();
}

/*****************************************************
 *
 *      Reset Registration
 *
 *****************************************************/
void itkRegistrator3D2D::ResetRegistration(void)
{
  cumulatedCorrectionTransform.set_identity();
}

/*****************************************************
 *
 *      Return the Transformation
 *
 *****************************************************/
const itkRegistrator3D2D::Transform3D & itkRegistrator3D2D::GetTransformation( void ) const
{
  return cumulatedCorrectionTransform;
}

/*****************************************************
 *
 *      Return the real number of iterations
 *
 *****************************************************/
unsigned int itkRegistrator3D2D::GetNumberOfIterationsPerformed( void ) const
{
  return numberOfIterations;
}

/*****************************************************
 *
 *      Allocate memory for internal arrays
 *
 *****************************************************/
void itkRegistrator3D2D::Allocate(void)
{

	const unsigned int numPoints = AssociatedPoints.size();
  const unsigned int column    = numPoints * 2;

  if( Jacobian )
  {
    delete Jacobian;
  }

  try
  {
    Jacobian = new vnl_matrix<double>(6,column);
  }
  catch(...)
  {
		throw itkRegistrator3D2DException("Registration Iterations: memory allocation for Jacobian");
  }

  if( PlanarError )
  {
    delete PlanarError;
  }

  try
  {
    PlanarError = new vnl_vector<double>(column);
  }
  catch(...)
  {
		throw itkRegistrator3D2DException("Registration Iterations: memory allocation for PlanarError");
  }

}

<|endoftext|>