<commit_before>//////////////////////////////////////////////////////////////////////
//
//                             Pixie
//
// Copyright  1999 - 2003, Okan Arikan
//
// Contact: okan@cs.utexas.edu
//
//	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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
//
//  File				:	fbw.cpp
//  Classes				:	CWinDisplay
//  Description			:	Implements a window that display the image
//
////////////////////////////////////////////////////////////////////////
#include "framebuffer.h"
#include "fbw.h"

// Foward declarations of functions included in this code module:
LRESULT CALLBACK	WndProc(HWND, UINT, WPARAM, LPARAM);

#define	color(r,g,b,a)	((a << 24) | (r << 16) | (g << 8) | b)
#define	get_a(col)		((col) >> 24)
#define	get_r(col)		(((col) >> 16) & 255)
#define	get_g(col)		(((col) >> 8) & 255)
#define	get_b(col)		((col) & 255)

///////////////////////////////////////////////////////////////////////
// Function				:	displayThread
// Description			:	Starts the display thread
// Return Value			:
// Comments				:
DWORD WINAPI  displayThread(void *w) {
	CWinDisplay	*cDisplay		=	(CWinDisplay *) w;

	// The thread main loop
	cDisplay->main();

	return 0;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	CWinDisplay
// Description			:	Ctor
// Return Value			:	-
// Comments				:
CWinDisplay::CWinDisplay(const char *name,const char *samples,int width,int height,int numSamples)
: CDisplay(name,samples,width,height,numSamples) {
	int	i,j;
	DWORD	threadID;

	hInst		=	NULL;
	hWnd		=	NULL;
	imageData	=	new unsigned int[width*height];

	// Create a checkerboard pattern
	for (i=0;i<height;i++) {
		for (j=0;j<width;j++) {
			unsigned char	d	=	255;
			int				t	=	0;

			if ((i & 63) < 32)	t ^=	1;
			if ((j & 63) < 32)	t ^=	1;

			if (t)			d	=	128;

			imageData[i*width+j]	=	color(d,d,d,d);
		}
	}

	active		=	TRUE;
	willRedraw	=	FALSE;

	// Create the thread
	thread		=	CreateThread(NULL,0,displayThread,this,0,&threadID);
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	~CWinDisplay
// Description			:	Dtor
// Return Value			:	-
// Comments				:
CWinDisplay::~CWinDisplay() {
	delete [] imageData;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	main
// Description			:	The main display loop
// Return Value			:	-
// Comments				:
void	CWinDisplay::main() {
	WNDCLASSEX	wcex;
	RECT		clientRect,windowRect;
	int			nw,nh;
	MSG			msg;

	hInst							= (HINSTANCE) GetModuleHandle(NULL);

	wcex.cbSize						= sizeof(WNDCLASSEX);

	wcex.style						= CS_HREDRAW | CS_VREDRAW;
	wcex.lpfnWndProc				= (WNDPROC)	WndProc;
	wcex.cbClsExtra					= 0;
	wcex.cbWndExtra					= 0;
	wcex.hInstance					= hInst;
	wcex.hIcon						= LoadIcon(NULL,IDI_APPLICATION);
	wcex.hCursor					= LoadCursor(NULL,IDC_ARROW);
	wcex.hbrBackground				= (HBRUSH)(COLOR_WINDOW+1);
	wcex.lpszMenuName				= NULL;
	wcex.lpszClassName				= "WinDisplay";
	wcex.hIconSm					= LoadIcon(NULL,IDI_APPLICATION);

	RegisterClassEx(&wcex);

	// Create the window
  hWnd = CreateWindow("WinDisplay", name, WS_OVERLAPPEDWINDOW, CW_USEDEFAULT,
    0, width, height, NULL, NULL, hInst, NULL);

	if (!hWnd)	{
		active	=	FALSE;
		return;	// Nothing to do
	}

  SetWindowLongPtr(hWnd, GWLP_USERDATA, (LONG_PTR)this);

	// Show the window
	ShowWindow(hWnd,SW_SHOW);
	UpdateWindow(hWnd);

	// Adjust the size of the window so that we display the entire picture
	GetWindowRect(hWnd,&windowRect);
	GetClientRect(hWnd,&clientRect);

	nw	=	width	+	(windowRect.right - windowRect.left) - (clientRect.right - clientRect.left);
	nh	=	height	+	(windowRect.bottom - windowRect.top) - (clientRect.bottom - clientRect.top);

	SetWindowPos(hWnd,NULL,0,0,nw,nh,SWP_SHOWWINDOW);

	// Set up the bitmap
	info.bmiHeader.biSize			= sizeof(BITMAPINFOHEADER);
	info.bmiHeader.biWidth			= width;
	info.bmiHeader.biHeight			= height;
	info.bmiHeader.biPlanes			= 1;
	info.bmiHeader.biBitCount		= 32;
	info.bmiHeader.biCompression	= BI_RGB;
	info.bmiHeader.biSizeImage		= 0;
	info.bmiHeader.biXPelsPerMeter	= 2834;
	info.bmiHeader.biYPelsPerMeter	= 2834;
	info.bmiHeader.biClrUsed		= 0;
	info.bmiHeader.biClrImportant	= 0;

	// Display the checkerboard
	redraw();

	// Main message loop:
	while (GetMessage(&msg, NULL, 0,0)) {
		TranslateMessage(&msg);
		DispatchMessage(&msg);
	}

	active	=	FALSE;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	redraw
// Description			:	Redraw the image
// Return Value			:	-
// Comments				:
void	CWinDisplay::redraw(HDC hdc, RECT *rcUpdate) {
  int x, y, w, h;

  if (rcUpdate) {
    // Repaint just the dirty rect.
    x = rcUpdate->left;
    y = rcUpdate->top;
    w = min(rcUpdate->right - rcUpdate->left, width);
    h = min(rcUpdate->bottom - rcUpdate->top, height);
  } else {
    // Repaint the whole thing.
    x = 0;
    y = 0;
    w = width;
    h = height;
  }

  // NOTE: Refer to Petzold's "Programming Windows" for an explanation of
  // the strange coordinate system bitmaps and the following function use.
  SetDIBitsToDevice(hdc, x, y, w, h, x, height - y - h, 0, height,
    imageData, &info, DIB_RGB_COLORS);

  willRedraw = FALSE;
}

// Helper function to repaint the whole window, out of the WM_PAINT message.
void CWinDisplay::redraw() {
  HDC hdc = GetDC(hWnd);
  redraw(hdc, NULL);  // Repaint the whole window.
  ReleaseDC(hWnd, hdc);
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	data
// Description			:	Draw the data onto the screen
// Return Value			:	-
// Comments				:
int	CWinDisplay::data(int x,int y,int w,int h,float *d) {
	int	i,j;

	clampData(w,h,d);

	for (i=0;i<h;i++) {
		const float		*src	=	&d[i*w*numSamples];
		unsigned int	*dest	=	&imageData[((height-(i+y)-1)*width+x)];

		switch(numSamples) {
		case 0:
			break;
		case 1:
			for (j=0;j<w;j++) {
				unsigned char	d	=	(unsigned char) (src[0]*255);

				*dest++				=	color(d,d,d,d);
				src++;
			}
			break;
		case 2:
			for (j=0;j<w;j++) {
				const float		r	=	src[0]*src[1]*255	+ (1-src[1])*get_r(dest[0]);
				const float		a	=	src[1]*255			+ (1-src[1])*get_a(dest[0]);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dr,dr,da);

				src					+=	2;
			}
			break;
		case 3:
			for (j=0;j<w;j++) {
				unsigned char	dr	=	(unsigned char) (src[0]*255);
				unsigned char	dg	=	(unsigned char) (src[1]*255);
				unsigned char	db	=	(unsigned char) (src[2]*255);

				*dest++				=	color(dr,dg,db,(unsigned char) 255);

				src					+=	3;
			}
			break;
		case 4:
			for (j=0;j<w;j++) {
				const float		r	=	src[0]*src[3]*255	+ (1-src[3])*get_r(dest[0]);
				const float		g	=	src[1]*src[3]*255	+ (1-src[3])*get_g(dest[0]);
				const float		b	=	src[2]*src[3]*255	+ (1-src[3])*get_b(dest[0]);
				const float		a	=	src[3]*255			+ (1-src[3])*get_a(dest[0]);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	dg	=	(unsigned char) g;
				unsigned char	db	=	(unsigned char) b;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dg,db,da);

				src					+=	4;
			}
			break;
		default:
			for (j=0;j<w;j++) {
				float			r	=	src[0]*src[3]*255	+ (1-src[3])*get_r(*dest);
				float			g	=	src[1]*src[3]*255	+ (1-src[3])*get_g(*dest);
				float			b	=	src[2]*src[3]*255	+ (1-src[3])*get_b(*dest);
				float			a	=	src[3]*255			+ (1-src[3])*get_a(*dest);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	dg	=	(unsigned char) g;
				unsigned char	db	=	(unsigned char) b;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dg,db,da);

				src					+=	numSamples;
			}
			break;
		}
	}

	if (active) {
		if (willRedraw == FALSE) {
			// Pump messages
			willRedraw	=	TRUE;
			PostMessage(hWnd,WM_PAINT,(WPARAM) this,0);
		}
	}

	return active;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	finish
// Description			:	Finish displaying the data
// Return Value			:	-
// Comments				:
void	CWinDisplay::finish() {
	// Pump the redraw message
	redraw();

	// Wait for the user to close the window
	WaitForSingleObject(thread,INFINITE);
}

///////////////////////////////////////////////////////////////////////
// Method				:	WndProc
// Description			:	Windows callback function
// Return Value			:	-
// Comments				:
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {
  CWinDisplay *dpy = (CWinDisplay *)GetWindowLongPtr(hWnd, GWLP_USERDATA);

	switch (message) {
    case WM_PAINT: {
      PAINTSTRUCT ps;
      BeginPaint(hWnd, &ps);
      dpy->redraw(ps.hdc, &ps.rcPaint);
      EndPaint(hWnd, &ps);
      break;
    }
		case WM_DESTROY:
			PostQuitMessage(0);
			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
   }

   return 0;
}

<commit_msg>Reduced flicker for Win32 Framebuffer.<commit_after>//////////////////////////////////////////////////////////////////////
//
//                             Pixie
//
// Copyright  1999 - 2003, Okan Arikan
//
// Contact: okan@cs.utexas.edu
//
//	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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
//
//  File				:	fbw.cpp
//  Classes				:	CWinDisplay
//  Description			:	Implements a window that display the image
//
////////////////////////////////////////////////////////////////////////
#include "framebuffer.h"
#include "fbw.h"

// Foward declarations of functions included in this code module:
LRESULT CALLBACK	WndProc(HWND, UINT, WPARAM, LPARAM);

#define	color(r,g,b,a)	((a << 24) | (r << 16) | (g << 8) | b)
#define	get_a(col)		((col) >> 24)
#define	get_r(col)		(((col) >> 16) & 255)
#define	get_g(col)		(((col) >> 8) & 255)
#define	get_b(col)		((col) & 255)

///////////////////////////////////////////////////////////////////////
// Function				:	displayThread
// Description			:	Starts the display thread
// Return Value			:
// Comments				:
DWORD WINAPI  displayThread(void *w) {
	CWinDisplay	*cDisplay		=	(CWinDisplay *) w;

	// The thread main loop
	cDisplay->main();

	return 0;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	CWinDisplay
// Description			:	Ctor
// Return Value			:	-
// Comments				:
CWinDisplay::CWinDisplay(const char *name,const char *samples,int width,int height,int numSamples)
: CDisplay(name,samples,width,height,numSamples) {
	int	i,j;
	DWORD	threadID;

	hInst		=	NULL;
	hWnd		=	NULL;
	imageData	=	new unsigned int[width*height];

	// Create a checkerboard pattern
	for (i=0;i<height;i++) {
		for (j=0;j<width;j++) {
			unsigned char	d	=	255;
			int				t	=	0;

			if ((i & 63) < 32)	t ^=	1;
			if ((j & 63) < 32)	t ^=	1;

			if (t)			d	=	128;

			imageData[i*width+j]	=	color(d,d,d,d);
		}
	}

	active		=	TRUE;
	willRedraw	=	FALSE;

	// Create the thread
	thread		=	CreateThread(NULL,0,displayThread,this,0,&threadID);
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	~CWinDisplay
// Description			:	Dtor
// Return Value			:	-
// Comments				:
CWinDisplay::~CWinDisplay() {
	delete [] imageData;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	main
// Description			:	The main display loop
// Return Value			:	-
// Comments				:
void	CWinDisplay::main() {
	WNDCLASSEX	wcex;
	RECT		clientRect,windowRect;
	int			nw,nh;
	MSG			msg;

	hInst							= (HINSTANCE) GetModuleHandle(NULL);

	wcex.cbSize						= sizeof(WNDCLASSEX);

	wcex.style						= CS_HREDRAW | CS_VREDRAW;
	wcex.lpfnWndProc				= (WNDPROC)	WndProc;
	wcex.cbClsExtra					= 0;
	wcex.cbWndExtra					= 0;
	wcex.hInstance					= hInst;
	wcex.hIcon						= LoadIcon(NULL,IDI_APPLICATION);
	wcex.hCursor					= LoadCursor(NULL,IDC_ARROW);
	wcex.hbrBackground				= NULL;
	wcex.lpszMenuName				= NULL;
	wcex.lpszClassName				= "WinDisplay";
	wcex.hIconSm					= LoadIcon(NULL,IDI_APPLICATION);

	RegisterClassEx(&wcex);

  // Create the window (non-resizable).
  hWnd = CreateWindow("WinDisplay", name,
    WS_OVERLAPPEDWINDOW & ~(WS_THICKFRAME | WS_MAXIMIZEBOX), CW_USEDEFAULT,
    0, width, height, NULL, NULL, hInst, NULL);

	if (!hWnd)	{
		active	=	FALSE;
		return;	// Nothing to do
	}

  SetWindowLongPtr(hWnd, GWLP_USERDATA, (LONG_PTR)this);

	// Show the window
	ShowWindow(hWnd,SW_SHOW);
	UpdateWindow(hWnd);

	// Adjust the size of the window so that we display the entire picture
	GetWindowRect(hWnd,&windowRect);
	GetClientRect(hWnd,&clientRect);

	nw	=	width	+	(windowRect.right - windowRect.left) - (clientRect.right - clientRect.left);
	nh	=	height	+	(windowRect.bottom - windowRect.top) - (clientRect.bottom - clientRect.top);

	SetWindowPos(hWnd,NULL,0,0,nw,nh,SWP_SHOWWINDOW);

	// Set up the bitmap
	info.bmiHeader.biSize			= sizeof(BITMAPINFOHEADER);
	info.bmiHeader.biWidth			= width;
	info.bmiHeader.biHeight			= height;
	info.bmiHeader.biPlanes			= 1;
	info.bmiHeader.biBitCount		= 32;
	info.bmiHeader.biCompression	= BI_RGB;
	info.bmiHeader.biSizeImage		= 0;
	info.bmiHeader.biXPelsPerMeter	= 2834;
	info.bmiHeader.biYPelsPerMeter	= 2834;
	info.bmiHeader.biClrUsed		= 0;
	info.bmiHeader.biClrImportant	= 0;

	// Display the checkerboard
	redraw();

	// Main message loop:
	while (GetMessage(&msg, NULL, 0,0)) {
		TranslateMessage(&msg);
		DispatchMessage(&msg);
	}

	active	=	FALSE;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	redraw
// Description			:	Redraw the image
// Return Value			:	-
// Comments				:
void	CWinDisplay::redraw(HDC hdc, RECT *rcUpdate) {
  int x, y, w, h;

  if (rcUpdate) {
    // Repaint just the dirty rect.
    x = rcUpdate->left;
    y = rcUpdate->top;
    w = min(rcUpdate->right - rcUpdate->left, width);
    h = min(rcUpdate->bottom - rcUpdate->top, height);
  } else {
    // Repaint the whole thing.
    x = 0;
    y = 0;
    w = width;
    h = height;
  }

  // NOTE: Refer to Petzold's "Programming Windows" for an explanation of
  // the strange coordinate system bitmaps and the following function use.
  SetDIBitsToDevice(hdc, x, y, w, h, x, height - y - h, 0, height,
    imageData, &info, DIB_RGB_COLORS);

  willRedraw = FALSE;
}

// Helper function to repaint the whole window, out of the WM_PAINT message.
void CWinDisplay::redraw() {
  HDC hdc = GetDC(hWnd);
  redraw(hdc, NULL);  // Repaint the whole window.
  ReleaseDC(hWnd, hdc);
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	data
// Description			:	Draw the data onto the screen
// Return Value			:	-
// Comments				:
int	CWinDisplay::data(int x,int y,int w,int h,float *d) {
	int	i,j;

	clampData(w,h,d);

	for (i=0;i<h;i++) {
		const float		*src	=	&d[i*w*numSamples];
		unsigned int	*dest	=	&imageData[((height-(i+y)-1)*width+x)];

		switch(numSamples) {
		case 0:
			break;
		case 1:
			for (j=0;j<w;j++) {
				unsigned char	d	=	(unsigned char) (src[0]*255);

				*dest++				=	color(d,d,d,d);
				src++;
			}
			break;
		case 2:
			for (j=0;j<w;j++) {
				const float		r	=	src[0]*src[1]*255	+ (1-src[1])*get_r(dest[0]);
				const float		a	=	src[1]*255			+ (1-src[1])*get_a(dest[0]);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dr,dr,da);

				src					+=	2;
			}
			break;
		case 3:
			for (j=0;j<w;j++) {
				unsigned char	dr	=	(unsigned char) (src[0]*255);
				unsigned char	dg	=	(unsigned char) (src[1]*255);
				unsigned char	db	=	(unsigned char) (src[2]*255);

				*dest++				=	color(dr,dg,db,(unsigned char) 255);

				src					+=	3;
			}
			break;
		case 4:
			for (j=0;j<w;j++) {
				const float		r	=	src[0]*src[3]*255	+ (1-src[3])*get_r(dest[0]);
				const float		g	=	src[1]*src[3]*255	+ (1-src[3])*get_g(dest[0]);
				const float		b	=	src[2]*src[3]*255	+ (1-src[3])*get_b(dest[0]);
				const float		a	=	src[3]*255			+ (1-src[3])*get_a(dest[0]);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	dg	=	(unsigned char) g;
				unsigned char	db	=	(unsigned char) b;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dg,db,da);

				src					+=	4;
			}
			break;
		default:
			for (j=0;j<w;j++) {
				float			r	=	src[0]*src[3]*255	+ (1-src[3])*get_r(*dest);
				float			g	=	src[1]*src[3]*255	+ (1-src[3])*get_g(*dest);
				float			b	=	src[2]*src[3]*255	+ (1-src[3])*get_b(*dest);
				float			a	=	src[3]*255			+ (1-src[3])*get_a(*dest);
				unsigned char	dr	=	(unsigned char) r;
				unsigned char	dg	=	(unsigned char) g;
				unsigned char	db	=	(unsigned char) b;
				unsigned char	da	=	(unsigned char) a;

				*dest++				=	color(dr,dg,db,da);

				src					+=	numSamples;
			}
			break;
		}
	}

	if (active) {
		if (willRedraw == FALSE) {
			// Pump messages
			willRedraw	=	TRUE;
			PostMessage(hWnd,WM_PAINT,(WPARAM) this,0);
		}
	}

	return active;
}

///////////////////////////////////////////////////////////////////////
// Class				:	CWinDisplay
// Method				:	finish
// Description			:	Finish displaying the data
// Return Value			:	-
// Comments				:
void	CWinDisplay::finish() {
	// Pump the redraw message
	redraw();

	// Wait for the user to close the window
	WaitForSingleObject(thread,INFINITE);
}

///////////////////////////////////////////////////////////////////////
// Method				:	WndProc
// Description			:	Windows callback function
// Return Value			:	-
// Comments				:
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {
  CWinDisplay *dpy = (CWinDisplay *)GetWindowLongPtr(hWnd, GWLP_USERDATA);

	switch (message) {
    case WM_PAINT: {
      PAINTSTRUCT ps;
      BeginPaint(hWnd, &ps);
      dpy->redraw(ps.hdc, &ps.rcPaint);
      EndPaint(hWnd, &ps);
      break;
    }
		case WM_DESTROY:
			PostQuitMessage(0);
			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
   }

   return 0;
}

<|endoftext|>


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

	Author: Tiago Britto Lobão
	tiago.blobao@gmail.com
*/

/*
	Purpose: Control an integrated circuit
	Cirrus Logic - CS5490

	Used to measure electrical quantities

	MIT License

******************************************/

#include "CS5490.h"

/******* Init CS5490 *******/

//For Arduino & ESP8622
#if !(defined ARDUINO_NodeMCU_32S ) && !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) && !defined(ARDUINO_Node32s)
	CS5490::CS5490(float mclk, int rx, int tx){
		this->MCLK = mclk;
		this->cSerial = new SoftwareSerial(rx,tx);
	}
//For ESP32 AND MEGA
#else
	CS5490::CS5490(float mclk){
		this->MCLK = mclk;
		this->cSerial = &Serial2;
	}
#endif

void CS5490::begin(int baudRate){
	cSerial->begin(baudRate);
	delay(10); //Avoid Bugs on Arduino UNO
}

/**************************************************************/
/*                     PRIVATE METHODS                        */
/**************************************************************/

/******* Write a register by the serial communication *******/
/* data bytes pass by data variable from this class */

void CS5490::write(int page, int address, long value){

	uint8_t checksum = 0;
	for(int i=0; i<3; i++)
		checksum += 0xFF - checksum;

	//Select page and address
	uint8_t buffer = (pageByte | (uint8_t)page);
	cSerial->write(buffer);
	buffer = (writeByte | (uint8_t)address);
	cSerial->write(buffer);

	//Send information
	for(int i=0; i<3 ; i++){
		data[i] = value & 0x000000FF;
		cSerial->write(this->data[i]);
		value >>= 8;
	}
	//Calculate and send checksum
	buffer = 0xFF - data[0] - data[1] - data[2];
	cSerial->write(buffer);
}

/******* Read a register by the serial communication *******/
/* data bytes pass by data variable from this class */

void CS5490::read(int page, int address){

	this->clearSerialBuffer();

	uint8_t buffer = (pageByte | (uint8_t)page);
	cSerial->write(buffer);
	buffer = (readByte | (uint8_t)address);
	cSerial->write(buffer);

	//Wait for 3 bytes to arrive
	while(cSerial->available() < 3);
	for(int i=0; i<3; i++){
		data[i] = cSerial->read();
	}
}

/******* Give an instruction by the serial communication *******/

void CS5490::instruct(int value){
	uint8_t buffer = (instructionByte | (uint8_t)value);
	cSerial->write(buffer);
}

/******* Clears cSerial Buffer *******/
void CS5490::clearSerialBuffer(){
	while (cSerial->available()) cSerial->read();
}

/*
  Function: toDouble
  Transforms a 24 bit number to a double number for easy processing data

  Param:
  data[] => Array with size 3. Each uint8_t is an 8 byte number received from CS5490
  LSBpow => Exponent specified from datasheet of the less significant bit
  MSBoption => Information of most significant bit case. It can be only three values:
    MSBnull (1)  The MSB is a Don't Care bit
    MSBsigned (2) the MSB is a negative value, requiring a 2 complement conversion
    MSBunsigned (3) The MSB is a positive value, the default case.

*/
double CS5490::toDouble(int LSBpow, int MSBoption){

	uint32_t buffer = 0;
	double output = 0.0;
	bool MSB;

	//Concat bytes in a 32 bit word
	buffer += this->data[0];
	buffer += this->data[1] << 8;
	buffer += this->data[2] << 16;

  switch(MSBoption){

    case MSBnull:
      this->data[2] &= ~(1 << 7); //Clear MSB
      buffer += this->data[2] << 16;
      output = (double)buffer;
      output /= pow(2,LSBpow);
    break;

    case MSBsigned:
      MSB = data[2] & 0x80;
  		if(MSB){  //- (2 complement conversion)
  			buffer = ~buffer;
  			//Clearing the first 8 bits
  			for(int i=24; i<32; i++)
  			  buffer &= ~(1 << i);
  			output = (double)buffer + 1.0;
  			output /= -pow(2,LSBpow);
  		}
  		else{     //+
  		  output = (double)buffer;
  			output /= (pow(2,LSBpow)-1.0);
  		}
    break;

    default:
    case MSBunsigned:
      output = (double)buffer;
  		output /= pow(2,LSBpow);
    break;

  }

	return output;
}

/**************************************************************/
/*              PUBLIC METHODS - Instructions                 */
/**************************************************************/
void CS5490::reset(){
	this->instruct(1);
}

void CS5490::standby(){
	this->instruct(2);
}

void CS5490::wakeUp(){
	this->instruct(3);
}

void CS5490::singConv(){
	this->instruct(20);
}

void CS5490::contConv(){
	this->instruct(21);
}

void CS5490::haltConv(){
	this->instruct(24);
}

/**************************************************************/
/*       PUBLIC METHODS - Calibration and Configuration       */
/**************************************************************/

/* SET */
void CS5490::setBaudRate(long value){

	//Calculate the correct binary value
	uint32_t hexBR = ceil(value*0.5242880/MCLK);
	if (hexBR > 65535) hexBR = 65535;
	hexBR += 0x020000;

	this->write(0x80,0x07,hexBR);
	delay(100); //To avoid bugs from ESP32

	//Reset Serial communication from controller
	cSerial->end();
	cSerial->begin(value);
	delay(50); //Avoid bugs from Arduino MEGA
	return;
}

/* GET */
int CS5490::getGainI(){
	//Page 16, Address 33
	this->read(16,33);
	return this->toDouble(22,MSBunsigned);
}

long CS5490::getBaudRate(){
	this->read(0,7);
	uint32_t buffer = this->data[0];
	buffer += this->data[1] << 8;
	buffer += this->data[2] << 16;
	buffer -= 0x020000;
	return ( (buffer/0.5242880)*MCLK );
}

/**************************************************************/
/*              PUBLIC METHODS - Measurements                 */
/**************************************************************/

double CS5490::getPeakV(){
	//Page 0, Address 36
	this->read(0,36);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getPeakI(){
	//Page 0, Address 37
	this->read(0,37);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstI(){
	//Page 16, Address 2
	this->read(16,2);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstV(){
	//Page 16, Address 3
	this->read(16,3);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstP(){
	//Page 16, Address 4
	this->read(16,4);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getRmsI(){
	//Page 16, Address 6
	this->read(16,6);
	return this->toDouble(24, MSBunsigned);
}

double CS5490::getRmsV(){
	//Page 16, Address 7
	this->read(16,7);
	return this->toDouble(24, MSBunsigned);
}

double CS5490::getAvgP(){
	//Page 16, Address 5
	this->read(16,5);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getAvgQ(){
	//Page 16, Address 14
	this->read(16,14);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getAvgS(){
	//Page 16, Address 20
	this->read(16,20);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstQ(){
	//Page 16, Address 15
	this->read(16,15);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getPF(){
	//Page 16, Address 21
	this->read(16,21);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalP(){
	//Page 16, Address 29
	this->read(16,29);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalS(){
	//Page 16, Address 30
	this->read(16,30);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalQ(){
	//Page 16, Address 31
	this->read(16,31);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getFreq(){
	//Page 16, Address 49
	this->read(16,49);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTime(){
	//Page 16, Address 61
	this->read(16,61);
	return this->toDouble(0, MSBunsigned);
}

/**************************************************************/
/*              PUBLIC METHODS - Read Register                */
/**************************************************************/

long CS5490::readReg(int page, int address){
	long value = 0;
	this->read(page, address);
	for(int i=0; i<3; i++){
		value += data[2-i];
		value <<= 8;
	}
	return value;
}
<commit_msg>Fixing readReg method<commit_after>/******************************************

	Author: Tiago Britto Lobão
	tiago.blobao@gmail.com
*/

/*
	Purpose: Control an integrated circuit
	Cirrus Logic - CS5490

	Used to measure electrical quantities

	MIT License

******************************************/

#include "CS5490.h"

/******* Init CS5490 *******/

//For Arduino & ESP8622
#if !(defined ARDUINO_NodeMCU_32S ) && !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) && !defined(ARDUINO_Node32s)
	CS5490::CS5490(float mclk, int rx, int tx){
		this->MCLK = mclk;
		this->cSerial = new SoftwareSerial(rx,tx);
	}
//For ESP32 AND MEGA
#else
	CS5490::CS5490(float mclk){
		this->MCLK = mclk;
		this->cSerial = &Serial2;
	}
#endif

void CS5490::begin(int baudRate){
	cSerial->begin(baudRate);
	delay(10); //Avoid Bugs on Arduino UNO
}

/**************************************************************/
/*                     PRIVATE METHODS                        */
/**************************************************************/

/******* Write a register by the serial communication *******/
/* data bytes pass by data variable from this class */

void CS5490::write(int page, int address, long value){

	uint8_t checksum = 0;
	for(int i=0; i<3; i++)
		checksum += 0xFF - checksum;

	//Select page and address
	uint8_t buffer = (pageByte | (uint8_t)page);
	cSerial->write(buffer);
	buffer = (writeByte | (uint8_t)address);
	cSerial->write(buffer);

	//Send information
	for(int i=0; i<3 ; i++){
		data[i] = value & 0x000000FF;
		cSerial->write(this->data[i]);
		value >>= 8;
	}
	//Calculate and send checksum
	buffer = 0xFF - data[0] - data[1] - data[2];
	cSerial->write(buffer);
}

/******* Read a register by the serial communication *******/
/* data bytes pass by data variable from this class */

void CS5490::read(int page, int address){

	this->clearSerialBuffer();

	uint8_t buffer = (pageByte | (uint8_t)page);
	cSerial->write(buffer);
	buffer = (readByte | (uint8_t)address);
	cSerial->write(buffer);

	//Wait for 3 bytes to arrive
	while(cSerial->available() < 3);
	for(int i=0; i<3; i++){
		data[i] = cSerial->read();
	}
}

/******* Give an instruction by the serial communication *******/

void CS5490::instruct(int value){
	uint8_t buffer = (instructionByte | (uint8_t)value);
	cSerial->write(buffer);
}

/******* Clears cSerial Buffer *******/
void CS5490::clearSerialBuffer(){
	while (cSerial->available()) cSerial->read();
}

/*
  Function: toDouble
  Transforms a 24 bit number to a double number for easy processing data

  Param:
  data[] => Array with size 3. Each uint8_t is an 8 byte number received from CS5490
  LSBpow => Exponent specified from datasheet of the less significant bit
  MSBoption => Information of most significant bit case. It can be only three values:
    MSBnull (1)  The MSB is a Don't Care bit
    MSBsigned (2) the MSB is a negative value, requiring a 2 complement conversion
    MSBunsigned (3) The MSB is a positive value, the default case.

*/
double CS5490::toDouble(int LSBpow, int MSBoption){

	uint32_t buffer = 0;
	double output = 0.0;
	bool MSB;

	//Concat bytes in a 32 bit word
	buffer += this->data[0];
	buffer += this->data[1] << 8;
	buffer += this->data[2] << 16;

  switch(MSBoption){

    case MSBnull:
      this->data[2] &= ~(1 << 7); //Clear MSB
      buffer += this->data[2] << 16;
      output = (double)buffer;
      output /= pow(2,LSBpow);
    break;

    case MSBsigned:
      MSB = data[2] & 0x80;
  		if(MSB){  //- (2 complement conversion)
  			buffer = ~buffer;
  			//Clearing the first 8 bits
  			for(int i=24; i<32; i++)
  			  buffer &= ~(1 << i);
  			output = (double)buffer + 1.0;
  			output /= -pow(2,LSBpow);
  		}
  		else{     //+
  		  output = (double)buffer;
  			output /= (pow(2,LSBpow)-1.0);
  		}
    break;

    default:
    case MSBunsigned:
      output = (double)buffer;
  		output /= pow(2,LSBpow);
    break;

  }

	return output;
}

/**************************************************************/
/*              PUBLIC METHODS - Instructions                 */
/**************************************************************/
void CS5490::reset(){
	this->instruct(1);
}

void CS5490::standby(){
	this->instruct(2);
}

void CS5490::wakeUp(){
	this->instruct(3);
}

void CS5490::singConv(){
	this->instruct(20);
}

void CS5490::contConv(){
	this->instruct(21);
}

void CS5490::haltConv(){
	this->instruct(24);
}

/**************************************************************/
/*       PUBLIC METHODS - Calibration and Configuration       */
/**************************************************************/

/* SET */
void CS5490::setBaudRate(long value){

	//Calculate the correct binary value
	uint32_t hexBR = ceil(value*0.5242880/MCLK);
	if (hexBR > 65535) hexBR = 65535;
	hexBR += 0x020000;

	this->write(0x80,0x07,hexBR);
	delay(100); //To avoid bugs from ESP32

	//Reset Serial communication from controller
	cSerial->end();
	cSerial->begin(value);
	delay(50); //Avoid bugs from Arduino MEGA
	return;
}

/* GET */
int CS5490::getGainI(){
	//Page 16, Address 33
	this->read(16,33);
	return this->toDouble(22,MSBunsigned);
}

long CS5490::getBaudRate(){
	this->read(0,7);
	uint32_t buffer = this->data[0];
	buffer += this->data[1] << 8;
	buffer += this->data[2] << 16;
	buffer -= 0x020000;
	return ( (buffer/0.5242880)*MCLK );
}

/**************************************************************/
/*              PUBLIC METHODS - Measurements                 */
/**************************************************************/

double CS5490::getPeakV(){
	//Page 0, Address 36
	this->read(0,36);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getPeakI(){
	//Page 0, Address 37
	this->read(0,37);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstI(){
	//Page 16, Address 2
	this->read(16,2);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstV(){
	//Page 16, Address 3
	this->read(16,3);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstP(){
	//Page 16, Address 4
	this->read(16,4);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getRmsI(){
	//Page 16, Address 6
	this->read(16,6);
	return this->toDouble(24, MSBunsigned);
}

double CS5490::getRmsV(){
	//Page 16, Address 7
	this->read(16,7);
	return this->toDouble(24, MSBunsigned);
}

double CS5490::getAvgP(){
	//Page 16, Address 5
	this->read(16,5);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getAvgQ(){
	//Page 16, Address 14
	this->read(16,14);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getAvgS(){
	//Page 16, Address 20
	this->read(16,20);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getInstQ(){
	//Page 16, Address 15
	this->read(16,15);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getPF(){
	//Page 16, Address 21
	this->read(16,21);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalP(){
	//Page 16, Address 29
	this->read(16,29);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalS(){
	//Page 16, Address 30
	this->read(16,30);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTotalQ(){
	//Page 16, Address 31
	this->read(16,31);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getFreq(){
	//Page 16, Address 49
	this->read(16,49);
	return this->toDouble(23, MSBsigned);
}

double CS5490::getTime(){
	//Page 16, Address 61
	this->read(16,61);
	return this->toDouble(0, MSBunsigned);
}

/**************************************************************/
/*              PUBLIC METHODS - Read Register                */
/**************************************************************/

long CS5490::readReg(int page, int address){
	uint32_t value = 0;
	this->read(page, address);
	value = value + data[2] << 8;
	value = value + data[1] << 8;
	value = value + data[0];
	return value;
}
<|endoftext|>


<commit_before>// This is a model of the 'Real' cryoEDM sourcetube - 25/01/2010
#include "TGeoManager.h"
#include "TGLViewer.h"
#include "TGLCamera.h"
#include "TGLPerspectiveCamera.h"

#include "../include/Units.h"
#include "../include/Constants.h"
#include "../include/Materials.h"

Bool_t Build_Geom(const TGeoManager* geoManager);
Bool_t Draw_Geom(const TGeoManager* geoManager);

namespace ModelParameters {
   // -- Ramsey Cell
   const Double_t ramseyCellRMin = 0.;
   const Double_t ramseyCellRMax = 235.0*Units::mm;
   const Double_t ramseyCellHalfLength = 120.*Units::mm;
   const Double_t ramseyCellAngle = 90.0;

   const Double_t bFieldX = 0.;
   const Double_t bFieldY = 0.;
   const Double_t bFieldZ = 5.0*Units::uT;

}

using namespace ModelParameters;

//__________________________________________________________________________
Int_t ramseycell_geom()
{
   // Create the geoManager
   TGeoManager* geoManager = new TGeoManager("GeoManager","Geometry Manager");
   // Build and write to file the simulation and visualisation geoms
   Build_Geom(geoManager);
   Draw_Geom(geoManager);

   return 0;
}

//__________________________________________________________________________
Bool_t Build_Geom(const TGeoManager* geoManager)
{
   // -------------------------------------
   // BUILDING GEOMETRY
   // Materials - Define the materials used. Leave the neutron properties to be defined on a run-by-run basis

   Materials::BuildMaterials(geoManager);
   TGeoMedium* beryllium = geoManager->GetMedium("Beryllium");
   TGeoMedium* blackhole = geoManager->GetMedium("BlackHole");
   TGeoMedium* heliumII = geoManager->GetMedium("HeliumII");

   // -------------------------------------
   // -- Making Top Volume
   TUCNGeoBBox* topShape = new TUCNGeoBBox("Top",100,100,100);
   TUCNBlackHole* top = new TUCNBlackHole("Top", topShape, blackhole);
   geoManager->SetTopVolume(top);
   top->SetVisibility(kFALSE);

   // -- Make the boundary volume in which all the others sit
   // -- This is what we will be reflecting off all the time
   Double_t surfaceRoughness = 0.05;
   TUCNGeoBBox* chamberShape = new TUCNGeoBBox("Chamber",10,10,10);
   TUCNBoundary* chamber = new TUCNBoundary("Chamber", chamberShape, beryllium, surfaceRoughness);
   chamber->SetLineColor(kOrange-7);
   chamber->SetLineWidth(1);
   chamber->SetVisibility(kFALSE);
   chamber->SetTransparency(80);
   // Add to geom
   top->AddNode(chamber,1);

   // -------------------------------------
   // -- Build Ramsey-Cell Volume
   TUCNGeoTube *ramseyCellShape = new TUCNGeoTube("RamseyCell", ramseyCellRMin, ramseyCellRMax, ramseyCellHalfLength);
   TUCNTrackingVolume* ramseyCell = new TUCNTrackingVolume("RamseyCell", ramseyCellShape, heliumII);
   ramseyCell->SetLineColor(kAzure-4);
   ramseyCell->SetLineWidth(1);
   ramseyCell->SetVisibility(kTRUE);
   ramseyCell->SetTransparency(20);

   // -- Define SourceTube matrix
   // Rotation seems to be applied before translation so bear that in mind when choosing which
   // coordinate to transform in translation
   TGeoRotation ramseyCellRot("ramseyCellRot",0,ramseyCellAngle,0); // phi, theta, psi
   TGeoTranslation ramseyCellTra("ramseyCellTra",0.,0.,0.); // x, y, z
   TGeoCombiTrans ramseyCellCom(ramseyCellTra,ramseyCellRot);
   TGeoHMatrix ramseyCellMat = ramseyCellCom;
   Char_t ramseyCellName[14];
   sprintf(ramseyCellName, "RamseyCellMat");
   ramseyCellMat.SetName(ramseyCellName);

   // -------------------------------------
   // -- Add Volume to Geometry
   chamber->AddNode(ramseyCell, 1, new TGeoHMatrix(ramseyCellMat));

   // -------------------------------------
   // -- Create and Attach Magnetic Field defined in local coordinate system
   TUCNMagField* magField = new TUCNUniformMagField("SolenoidField", bFieldX, bFieldY, bFieldZ);
   ramseyCell->AttachMagField(magField);

   // -------------------------------------
   // -- Close Geometry
   geoManager->CloseGeometry();

   // -------------------------------------
   // -- Write out geometry to file
   const char *fileName = "$(UCNSIM)/geom/ramseycell_geom.root";
   cerr << "Simulation Geometry Built... Writing to file: " << fileName << endl;
   geoManager->Export(fileName);

   return kTRUE;
}

//__________________________________________________________________________
Bool_t Draw_Geom(const TGeoManager* geoManager)
{
   // -------------------------------------
   // -- Draw the vis-geometry in OpenGLViewer
   TCanvas* canvas = new TCanvas("GeomCanvas","Canvas for visualisation of EDM Geom",60,40,600,600);
   canvas->cd();
   geoManager->GetTopVolume()->Draw("ogl");
   geoManager->SetVisLevel(4); // Default draws 4 levels down volume heirarchy
   geoManager->SetVisOption(0); // Default is 1, but 0 draws all the intermediate volumes not just the final bottom layer
   geoManager->ViewLeaves(kTRUE);

   // -- Get the GLViewer so we can manipulate the camera
   TGLViewer * glViewer = dynamic_cast<TGLViewer*>(gPad->GetViewer3D());
   // -- Select Draw style
   glViewer->SetStyle(TGLRnrCtx::kFill); // TGLRnrCtx::kFill, TGLRnrCtx::kOutline, TGLRnrCtx::kWireFrame
   // -- Set Background colour
   glViewer->SetClearColor(TColor::kWhite);
   // -- Set Lights - turn some off if you wish
// TGLLightSet* lightSet = glViewer->GetLightSet();
// lightSet->SetLight(TGLLightSet::kLightLeft, kFALSE);
   // -- Set Camera type
   // kCameraPerspXOZ, kCameraPerspYOZ, kCameraPerspXOY, kCameraOrthoXOY
   // kCameraOrthoXOZ, kCameraOrthoZOY, kCameraOrthoXnOY, kCameraOrthoXnOZ, kCameraOrthoZnOY
   TGLViewer::ECameraType camera = 2;
   glViewer->SetCurrentCamera(camera);
   glViewer->CurrentCamera().SetExternalCenter(kTRUE);
   Double_t cameraCentre[3] = {0., 0., 0.};
   glViewer->SetPerspectiveCamera(camera,4,100,&cameraCentre[0],0,0);
   // -- Draw Reference Point, Axes
   Double_t refPoint[3] = {0.,0.,0.};
   // Int_t axesType = 0(Off), 1(EDGE), 2(ORIGIN), Bool_t axesDepthTest, Bool_t referenceOn, const Double_t referencePos[3]
   glViewer->SetGuideState(0, kFALSE, kFALSE, refPoint);
   glViewer->UpdateScene();
   return kTRUE;
}

<commit_msg>Cleaned up some text in RamseyCell macro<commit_after>// This is a model of the 'Real' cryoEDM sourcetube - 25/01/2010
#include "TGeoManager.h"
#include "TGLViewer.h"
#include "TGLCamera.h"
#include "TGLPerspectiveCamera.h"

#include "../include/Units.h"
#include "../include/Constants.h"
#include "../include/Materials.h"

Bool_t Build_Geom(const TGeoManager* geoManager);
Bool_t Draw_Geom(const TGeoManager* geoManager);

namespace ModelParameters {
   // -- Ramsey Cell
   const Double_t ramseyCellRMin = 0.;
   const Double_t ramseyCellRMax = 235.0*Units::mm;
   const Double_t ramseyCellHalfLength = 120.*Units::mm;
   const Double_t ramseyCellAngle = 90.0;

   const Double_t bFieldX = 0.;
   const Double_t bFieldY = 0.;
   const Double_t bFieldZ = 5.0*Units::uT;

}

using namespace ModelParameters;

//__________________________________________________________________________
Int_t ramseycell_geom()
{
   // Create the geoManager
   TGeoManager* geoManager = new TGeoManager("GeoManager","Geometry Manager");
   // Build and write to file the simulation and visualisation geoms
   Build_Geom(geoManager);
   Draw_Geom(geoManager);

   return 0;
}

//__________________________________________________________________________
Bool_t Build_Geom(const TGeoManager* geoManager)
{
   // -------------------------------------
   // *** BUILDING GEOMETRY
   // -------------------------------------
   // Materials - Define the materials used.
   // Leave the neutron properties to be defined on a run-by-run basis

   Materials::BuildMaterials(geoManager);
   TGeoMedium* beryllium = geoManager->GetMedium("Beryllium");
   TGeoMedium* blackhole = geoManager->GetMedium("BlackHole");
   TGeoMedium* heliumII = geoManager->GetMedium("HeliumII");

   // -------------------------------------
   // -- Making Top Volume
   TUCNGeoBBox* topShape = new TUCNGeoBBox("Top",100,100,100);
   TUCNBlackHole* top = new TUCNBlackHole("Top", topShape, blackhole);
   geoManager->SetTopVolume(top);
   top->SetVisibility(kFALSE);

   // -- Make the boundary volume in which all the others sit
   // -- This is what we will be reflecting off all the time
   Double_t surfaceRoughness = 0.05;
   TUCNGeoBBox* chamberShape = new TUCNGeoBBox("Chamber",10,10,10);
   TUCNBoundary* chamber = new TUCNBoundary("Chamber", chamberShape, beryllium, surfaceRoughness);
   chamber->SetLineColor(kOrange-7);
   chamber->SetLineWidth(1);
   chamber->SetVisibility(kFALSE);
   chamber->SetTransparency(80);
   // Add to geom
   top->AddNode(chamber,1);

   // -------------------------------------
   // -- Build Ramsey-Cell Volume
   TUCNGeoTube *ramseyCellShape = new TUCNGeoTube("RamseyCell", ramseyCellRMin, ramseyCellRMax, ramseyCellHalfLength);
   TUCNTrackingVolume* ramseyCell = new TUCNTrackingVolume("RamseyCell", ramseyCellShape, heliumII);
   ramseyCell->SetLineColor(kAzure-4);
   ramseyCell->SetLineWidth(1);
   ramseyCell->SetVisibility(kTRUE);
   ramseyCell->SetTransparency(20);

   // -- Define SourceTube matrix
   // Rotation seems to be applied before translation so bear that in mind when choosing which
   // coordinate to transform in translation
   TGeoRotation ramseyCellRot("ramseyCellRot",0,ramseyCellAngle,0); // phi, theta, psi
   TGeoTranslation ramseyCellTra("ramseyCellTra",0.,0.,0.); // x, y, z
   TGeoCombiTrans ramseyCellCom(ramseyCellTra,ramseyCellRot);
   TGeoHMatrix ramseyCellMat = ramseyCellCom;
   Char_t ramseyCellName[14];
   sprintf(ramseyCellName, "RamseyCellMat");
   ramseyCellMat.SetName(ramseyCellName);

   // -------------------------------------
   // -- Add Volume to Geometry
   chamber->AddNode(ramseyCell, 1, new TGeoHMatrix(ramseyCellMat));

   // -------------------------------------
   // *** CREATING MAGFIELDS
   // -------------------------------------
   // -- Create and Attach Magnetic Field defined in local coordinate system
   TUCNMagField* magField = new TUCNUniformMagField("SolenoidField", bFieldX, bFieldY, bFieldZ);
   ramseyCell->AttachMagField(magField);

   // -------------------------------------
   // -- Close Geometry
   geoManager->CloseGeometry();

   // -------------------------------------
   // -- Write out geometry to file
   const char *fileName = "$(UCNSIM)/geom/ramseycell_geom.root";
   cerr << "Simulation Geometry Built... Writing to file: " << fileName << endl;
   geoManager->Export(fileName);

   return kTRUE;
}

//__________________________________________________________________________
Bool_t Draw_Geom(const TGeoManager* geoManager)
{
   // -------------------------------------
   // -- Draw the vis-geometry in OpenGLViewer
   TCanvas* canvas = new TCanvas("GeomCanvas","Canvas for visualisation of EDM Geom",60,40,600,600);
   canvas->cd();
   geoManager->GetTopVolume()->Draw("ogl");
   geoManager->SetVisLevel(4); // Default draws 4 levels down volume heirarchy
   geoManager->SetVisOption(0); // Default is 1, but 0 draws all the intermediate volumes not just the final bottom layer
   geoManager->ViewLeaves(kTRUE);

   // -- Get the GLViewer so we can manipulate the camera
   TGLViewer * glViewer = dynamic_cast<TGLViewer*>(gPad->GetViewer3D());
   // -- Select Draw style
   glViewer->SetStyle(TGLRnrCtx::kFill); // TGLRnrCtx::kFill, TGLRnrCtx::kOutline, TGLRnrCtx::kWireFrame
   // -- Set Background colour
   glViewer->SetClearColor(TColor::kWhite);
   // -- Set Lights - turn some off if you wish
// TGLLightSet* lightSet = glViewer->GetLightSet();
// lightSet->SetLight(TGLLightSet::kLightLeft, kFALSE);
   // -- Set Camera type
   // kCameraPerspXOZ, kCameraPerspYOZ, kCameraPerspXOY, kCameraOrthoXOY
   // kCameraOrthoXOZ, kCameraOrthoZOY, kCameraOrthoXnOY, kCameraOrthoXnOZ, kCameraOrthoZnOY
   TGLViewer::ECameraType camera = 2;
   glViewer->SetCurrentCamera(camera);
   glViewer->CurrentCamera().SetExternalCenter(kTRUE);
   Double_t cameraCentre[3] = {0., 0., 0.};
   glViewer->SetPerspectiveCamera(camera,4,100,&cameraCentre[0],0,0);
   // -- Draw Reference Point, Axes
   Double_t refPoint[3] = {0.,0.,0.};
   // Int_t axesType = 0(Off), 1(EDGE), 2(ORIGIN), Bool_t axesDepthTest, Bool_t referenceOn, const Double_t referencePos[3]
   glViewer->SetGuideState(0, kFALSE, kFALSE, refPoint);
   glViewer->UpdateScene();
   return kTRUE;
}

<|endoftext|>


<commit_before>/* bzflag
 * Copyright (c) 1993-2010 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

// interface header
#include "NetMessage.h"

// system headers
#include <assert.h>
#include <string.h>

// common headers
#include "bzfio.h"
#include "Pack.h"

NetMessage::SendFunc      NetMessage::sendFunc      = NULL;
NetMessage::BroadcastFunc NetMessage::broadcastFunc = NULL;

const size_t NetMessage::lenCodeOffset = 2 * sizeof(uint16_t);
const size_t NetMessage::capacityStep  = 256;
const size_t NetMessage::capacityDefault = NetMessage::capacityStep;

//============================================================================//

uint16_t NetRecvMsg::getCode() const
{
  uint16_t code;
  nboUnpackUInt16((uint16_t*)data + 1, code);
  return code;
}

//============================================================================//

NetMessage::NetMessage()
: data(NULL)
{
  clear(); // setup the initial capacity
}

NetMessage::NetMessage(size_t size)
{
  capacity = size + lenCodeOffset;;
  dataSize = capacity;
  data = (char*) malloc(dataSize);
  memset(data, 0, lenCodeOffset);
  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(const void* fullData, size_t fullSize)
{
  assert(fullSize >= lenCodeOffset);

  const uint16_t msgSize = *((uint16_t*)fullData);
  assert(msgSize == (fullSize - lenCodeOffset));

  dataSize = fullSize;
  capacity = fullSize;
  data = (char*) malloc(dataSize);
  memcpy(data, fullData, dataSize);
  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(uint16_t len, uint16_t code, const void* msgData)
{
  dataSize = lenCodeOffset + len;
  capacity = dataSize;
  data = (char*) malloc(dataSize);

  uint16_t* u16ptr = (uint16_t*) data;
  nboPackUInt16(u16ptr + 0, len);
  nboPackUInt16(u16ptr + 1, code);
  memcpy(data + lenCodeOffset, msgData, len);

  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(const NetMessage &msg)
: dataSize(msg.dataSize)
, capacity(msg.capacity)
, readIndex(msg.readIndex)
{
  data = (char*) malloc(dataSize);
  memcpy(data, msg.data, dataSize);
}

NetMessage& NetMessage::operator=(const NetMessage &msg)
{
  free(data);

  dataSize  = msg.dataSize;
  capacity  = msg.capacity;
  readIndex = msg.readIndex;

  data = (char*) malloc(dataSize);
  memcpy(data, msg.data, dataSize);

  return *this;
}

NetMessage::~NetMessage()
{
  free(data);
}

//============================================================================//

void NetMessage::clear()
{
  free(data);
  capacity = capacityDefault;
  data = (char*) malloc(capacity);
  memset(data, 0, lenCodeOffset);
  dataSize  = lenCodeOffset;
  readIndex = lenCodeOffset;
}

void NetMessage::growCapacity(size_t s)
{
  if (s < capacity) {
    return;
  }

  size_t newCapacity = capacity;
  do {
    newCapacity += capacityStep;
  }
  while (newCapacity < s);

  capacity = newCapacity;

  data = (char*) realloc(data, capacity);
  if (data == NULL) {
    logDebugMessage(0,
      "ERROR: NetMessage::growCapacity(%i) out-of-memory\n", (int) s);
    exit(2);
  }
}

void NetMessage::checkData(size_t s)
{
  growCapacity(capacity + s);
}

inline char* NetMessage::getWriteBuffer()
{
  return data + dataSize;
}

inline char* NetMessage::checkReadBuffer(size_t s)
{
  if ((readIndex + s) > dataSize) {
    return NULL;
  }
  return data + readIndex;
}

//============================================================================//

void NetMessage::send(NetHandler *dst, uint16_t messageCode)
{
  if (!sendFunc) {
    logDebugMessage(0, "NetMessage::send()  sendFunc == NULL\n");
  }

  if (!dst) {
    return;
  }

  // setup the length & code
  nboPackUInt16(data, uint16_t(dataSize - lenCodeOffset));
  nboPackUInt16(data + sizeof(uint16_t), messageCode);

  sendFunc(dst, data, dataSize);
}

void NetMessage::broadcast(uint16_t messageCode, bool toTextClients)
{
  if (!broadcastFunc) {
    logDebugMessage(0, "NetMessage::broadcast()  broadcastFunc == NULL\n");
  }

  // setup the length & code
  nboPackUInt16(data, uint16_t(dataSize - lenCodeOffset));
  nboPackUInt16(data + sizeof(uint16_t), messageCode);

  broadcastFunc(data, dataSize, toTextClients);
}

//============================================================================//
//
//  Packing routines
//

void NetMessage::packInt8(int8_t val)
{
  checkData(sizeof(int8_t));
  nboPackInt8(getWriteBuffer(), val);
  dataSize += sizeof(int8_t);
}

void NetMessage::packInt16(int16_t val)
{
  checkData(sizeof(int16_t));
  nboPackInt16(getWriteBuffer(), val);
  dataSize += sizeof(int16_t);
}

void NetMessage::packInt32(int32_t val)
{
  checkData(sizeof(int32_t));
  nboPackInt32(getWriteBuffer(), val);
  dataSize += sizeof(int32_t);
}

void NetMessage::packInt64(int64_t val)
{
  checkData(sizeof(int64_t));
  nboPackInt64(getWriteBuffer(), val);
  dataSize += sizeof(int64_t);
}

void NetMessage::packUInt8(uint8_t val)
{
  checkData(sizeof(uint8_t));
  nboPackUInt8(getWriteBuffer(), val);
  dataSize += sizeof(uint8_t);
}

void NetMessage::packUInt16(uint16_t val)
{
  checkData(sizeof(uint16_t));
  nboPackUInt16(getWriteBuffer(), val);
  dataSize += sizeof(uint16_t);
}

void NetMessage::packUInt32(uint32_t val)
{
  checkData(sizeof(uint32_t));
  nboPackUInt32(getWriteBuffer(), val);
  dataSize += sizeof(uint32_t);
}

void NetMessage::packUInt64(uint64_t val)
{
  checkData(sizeof(uint64_t));
  nboPackUInt64(getWriteBuffer(), val);
  dataSize += sizeof(uint64_t);
}

void NetMessage::packFloat(float val)
{
  checkData(sizeof(float));
  nboPackFloat(getWriteBuffer(), val);
  dataSize += sizeof(float);
}

void NetMessage::packDouble(double val)
{
  checkData(sizeof(double));
  nboPackDouble(getWriteBuffer(), val);
  dataSize += sizeof(double);
}

void NetMessage::packFVec2(const fvec2& val)
{
  checkData(sizeof(fvec2));
  nboPackFVec2(getWriteBuffer(), val);
  dataSize += sizeof(fvec2);
}

void NetMessage::packFVec3(const fvec3& val)
{
  checkData(sizeof(fvec3));
  nboPackFVec3(getWriteBuffer(), val);
  dataSize += sizeof(fvec3);
}

void NetMessage::packFVec4(const fvec4& val)
{
  checkData(sizeof(fvec4));
  nboPackFVec4(getWriteBuffer(), val);
  dataSize += sizeof(fvec4);
}

void NetMessage::packString(const char* val, int len)
{
  checkData(len);
  nboPackString(getWriteBuffer(), val, len);
  dataSize += len;
}

void NetMessage::packStdString(const std::string& str)
{
  const size_t packSize = nboStdStringPackSize(str);
  checkData(packSize);
  nboPackStdString(getWriteBuffer(), str);
  dataSize += packSize;
}

//============================================================================//
//
//  Unpacking routines
//

int8_t NetMessage::unpackInt8()
{
  int8_t v = 0;
  char *p = checkReadBuffer(sizeof(int8_t));
  if (p) {
    nboUnpackInt8(p, v);
    readIndex += sizeof(int8_t);
  }
  return v;
}

int16_t NetMessage::unpackInt16()
{
  int16_t v = 0;
  char *p = checkReadBuffer(sizeof(int16_t));
  if (p) {
    nboUnpackInt16(p, v);
    readIndex += sizeof(int16_t);
  }
  return v;
}

int32_t NetMessage::unpackInt32()
{
  int32_t v = 0;
  char *p = checkReadBuffer(sizeof(int32_t));
  if (p) {
    nboUnpackInt32(p, v);
    readIndex += sizeof(int32_t);
  }
  return v;
}

int64_t NetMessage::unpackInt64()
{
  int64_t v = 0;
  char *p = checkReadBuffer(sizeof(int64_t));
  if (p) {
    nboUnpackInt64(p, v);
    readIndex += sizeof(int64_t);
  }
  return v;
}

uint8_t NetMessage::unpackUInt8()
{
  uint8_t v = 0;
  char *p = checkReadBuffer(sizeof(uint8_t));
  if (p) {
    nboUnpackUInt8(p, v);
    readIndex += sizeof(uint8_t);
  }
  return v;
}

uint16_t NetMessage::unpackUInt16()
{
  uint16_t v = 0;
  char *p = checkReadBuffer(sizeof(uint16_t));
  if (p) {
    nboUnpackUInt16(p, v);
    readIndex += sizeof(uint16_t);
  }
  return v;
}

uint32_t NetMessage::unpackUInt32()
{
  uint32_t v = 0;
  char *p = checkReadBuffer(sizeof(uint32_t));
  if (p) {
    nboUnpackUInt32(p, v);
    readIndex += sizeof(uint32_t);
  }
  return v;
}

uint64_t NetMessage::unpackUInt64()
{
  uint64_t v = 0;
  char *p = checkReadBuffer(sizeof(uint64_t));
  if (p) {
    nboUnpackUInt64(p, v);
    readIndex += sizeof(uint64_t);
  }
  return v;
}

float NetMessage::unpackFloat()
{
  float v = 0.0f;
  char *p = checkReadBuffer(sizeof(float));
  if (p) {
    nboUnpackFloat(p, v);
    readIndex += sizeof(float);
  }
  return v;
}

double NetMessage::unpackDouble()
{
  double v = 0.0;
  char *p = checkReadBuffer(sizeof(double));
  if (p) {
    nboUnpackDouble(p, v);
    readIndex += sizeof(double);
  }
  return v;
}

fvec2 NetMessage::unpackFVec2()
{
  fvec2 v(0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec2));
  if (p) {
    nboUnpackFVec2(p, v);
    readIndex += sizeof(fvec2);
  }
  return v;
}

fvec3 NetMessage::unpackFVec3()
{
  fvec3 v(0.0f, 0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec3));
  if (p) {
    nboUnpackFVec3(p, v);
    readIndex += sizeof(fvec3);
  }
  return v;
}

fvec4 NetMessage::unpackFVec4()
{
  fvec4 v(0.0f, 0.0f, 0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec4));
  if (p) {
    nboUnpackFVec4(p, v);
    readIndex += sizeof(fvec4);
  }
  return v;
}

std::string NetMessage::unpackStdString()
{
  std::string v;
  const uint32_t strSize = unpackUInt32();
  char *p = checkReadBuffer(strSize);
  if (p) {
    char* buffer = new char[strSize + 1];
    nboUnpackString(p, buffer, strSize);
    buffer[strSize] = 0;
    v = buffer;
    delete[] buffer;
    readIndex += strSize;
  }
  return v;
}

std::string NetMessage::unpackStdStringRaw()
{
  std::string v;
  const uint32_t strSize = unpackUInt32();
  char *p = checkReadBuffer(strSize);
  if (p) {
    char* buffer = new char[strSize];
    nboUnpackString(p, buffer, strSize);
    v.assign(buffer, strSize);
    delete[] buffer;
    readIndex += strSize;
  }
  return v;
}

void NetMessage::unpackString(char* buf, size_t len)
{
  char* p = checkReadBuffer(len);
  if (p) {
    memcpy(buf, p, len);
    readIndex += len;
  } else {
    memset(buf, 0, len);
  }
}

//============================================================================//

// Local Variables: ***
// mode: C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<commit_msg>Call assert() without involving an otherwise unused variable.<commit_after>/* bzflag
 * Copyright (c) 1993-2010 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

// interface header
#include "NetMessage.h"

// system headers
#include <assert.h>
#include <string.h>

// common headers
#include "bzfio.h"
#include "Pack.h"

NetMessage::SendFunc      NetMessage::sendFunc      = NULL;
NetMessage::BroadcastFunc NetMessage::broadcastFunc = NULL;

const size_t NetMessage::lenCodeOffset = 2 * sizeof(uint16_t);
const size_t NetMessage::capacityStep  = 256;
const size_t NetMessage::capacityDefault = NetMessage::capacityStep;

//============================================================================//

uint16_t NetRecvMsg::getCode() const
{
  uint16_t code;
  nboUnpackUInt16((uint16_t*)data + 1, code);
  return code;
}

//============================================================================//

NetMessage::NetMessage()
: data(NULL)
{
  clear(); // setup the initial capacity
}

NetMessage::NetMessage(size_t size)
{
  capacity = size + lenCodeOffset;;
  dataSize = capacity;
  data = (char*) malloc(dataSize);
  memset(data, 0, lenCodeOffset);
  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(const void* fullData, size_t fullSize)
{
  assert(fullSize >= lenCodeOffset);
  assert(*((uint16_t*)fullData) == (fullSize - lenCodeOffset));

  dataSize = fullSize;
  capacity = fullSize;
  data = (char*) malloc(dataSize);
  memcpy(data, fullData, dataSize);
  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(uint16_t len, uint16_t code, const void* msgData)
{
  dataSize = lenCodeOffset + len;
  capacity = dataSize;
  data = (char*) malloc(dataSize);

  uint16_t* u16ptr = (uint16_t*) data;
  nboPackUInt16(u16ptr + 0, len);
  nboPackUInt16(u16ptr + 1, code);
  memcpy(data + lenCodeOffset, msgData, len);

  readIndex = lenCodeOffset;
}

NetMessage::NetMessage(const NetMessage &msg)
: dataSize(msg.dataSize)
, capacity(msg.capacity)
, readIndex(msg.readIndex)
{
  data = (char*) malloc(dataSize);
  memcpy(data, msg.data, dataSize);
}

NetMessage& NetMessage::operator=(const NetMessage &msg)
{
  free(data);

  dataSize  = msg.dataSize;
  capacity  = msg.capacity;
  readIndex = msg.readIndex;

  data = (char*) malloc(dataSize);
  memcpy(data, msg.data, dataSize);

  return *this;
}

NetMessage::~NetMessage()
{
  free(data);
}

//============================================================================//

void NetMessage::clear()
{
  free(data);
  capacity = capacityDefault;
  data = (char*) malloc(capacity);
  memset(data, 0, lenCodeOffset);
  dataSize  = lenCodeOffset;
  readIndex = lenCodeOffset;
}

void NetMessage::growCapacity(size_t s)
{
  if (s < capacity) {
    return;
  }

  size_t newCapacity = capacity;
  do {
    newCapacity += capacityStep;
  }
  while (newCapacity < s);

  capacity = newCapacity;

  data = (char*) realloc(data, capacity);
  if (data == NULL) {
    logDebugMessage(0,
      "ERROR: NetMessage::growCapacity(%i) out-of-memory\n", (int) s);
    exit(2);
  }
}

void NetMessage::checkData(size_t s)
{
  growCapacity(capacity + s);
}

inline char* NetMessage::getWriteBuffer()
{
  return data + dataSize;
}

inline char* NetMessage::checkReadBuffer(size_t s)
{
  if ((readIndex + s) > dataSize) {
    return NULL;
  }
  return data + readIndex;
}

//============================================================================//

void NetMessage::send(NetHandler *dst, uint16_t messageCode)
{
  if (!sendFunc) {
    logDebugMessage(0, "NetMessage::send()  sendFunc == NULL\n");
  }

  if (!dst) {
    return;
  }

  // setup the length & code
  nboPackUInt16(data, uint16_t(dataSize - lenCodeOffset));
  nboPackUInt16(data + sizeof(uint16_t), messageCode);

  sendFunc(dst, data, dataSize);
}

void NetMessage::broadcast(uint16_t messageCode, bool toTextClients)
{
  if (!broadcastFunc) {
    logDebugMessage(0, "NetMessage::broadcast()  broadcastFunc == NULL\n");
  }

  // setup the length & code
  nboPackUInt16(data, uint16_t(dataSize - lenCodeOffset));
  nboPackUInt16(data + sizeof(uint16_t), messageCode);

  broadcastFunc(data, dataSize, toTextClients);
}

//============================================================================//
//
//  Packing routines
//

void NetMessage::packInt8(int8_t val)
{
  checkData(sizeof(int8_t));
  nboPackInt8(getWriteBuffer(), val);
  dataSize += sizeof(int8_t);
}

void NetMessage::packInt16(int16_t val)
{
  checkData(sizeof(int16_t));
  nboPackInt16(getWriteBuffer(), val);
  dataSize += sizeof(int16_t);
}

void NetMessage::packInt32(int32_t val)
{
  checkData(sizeof(int32_t));
  nboPackInt32(getWriteBuffer(), val);
  dataSize += sizeof(int32_t);
}

void NetMessage::packInt64(int64_t val)
{
  checkData(sizeof(int64_t));
  nboPackInt64(getWriteBuffer(), val);
  dataSize += sizeof(int64_t);
}

void NetMessage::packUInt8(uint8_t val)
{
  checkData(sizeof(uint8_t));
  nboPackUInt8(getWriteBuffer(), val);
  dataSize += sizeof(uint8_t);
}

void NetMessage::packUInt16(uint16_t val)
{
  checkData(sizeof(uint16_t));
  nboPackUInt16(getWriteBuffer(), val);
  dataSize += sizeof(uint16_t);
}

void NetMessage::packUInt32(uint32_t val)
{
  checkData(sizeof(uint32_t));
  nboPackUInt32(getWriteBuffer(), val);
  dataSize += sizeof(uint32_t);
}

void NetMessage::packUInt64(uint64_t val)
{
  checkData(sizeof(uint64_t));
  nboPackUInt64(getWriteBuffer(), val);
  dataSize += sizeof(uint64_t);
}

void NetMessage::packFloat(float val)
{
  checkData(sizeof(float));
  nboPackFloat(getWriteBuffer(), val);
  dataSize += sizeof(float);
}

void NetMessage::packDouble(double val)
{
  checkData(sizeof(double));
  nboPackDouble(getWriteBuffer(), val);
  dataSize += sizeof(double);
}

void NetMessage::packFVec2(const fvec2& val)
{
  checkData(sizeof(fvec2));
  nboPackFVec2(getWriteBuffer(), val);
  dataSize += sizeof(fvec2);
}

void NetMessage::packFVec3(const fvec3& val)
{
  checkData(sizeof(fvec3));
  nboPackFVec3(getWriteBuffer(), val);
  dataSize += sizeof(fvec3);
}

void NetMessage::packFVec4(const fvec4& val)
{
  checkData(sizeof(fvec4));
  nboPackFVec4(getWriteBuffer(), val);
  dataSize += sizeof(fvec4);
}

void NetMessage::packString(const char* val, int len)
{
  checkData(len);
  nboPackString(getWriteBuffer(), val, len);
  dataSize += len;
}

void NetMessage::packStdString(const std::string& str)
{
  const size_t packSize = nboStdStringPackSize(str);
  checkData(packSize);
  nboPackStdString(getWriteBuffer(), str);
  dataSize += packSize;
}

//============================================================================//
//
//  Unpacking routines
//

int8_t NetMessage::unpackInt8()
{
  int8_t v = 0;
  char *p = checkReadBuffer(sizeof(int8_t));
  if (p) {
    nboUnpackInt8(p, v);
    readIndex += sizeof(int8_t);
  }
  return v;
}

int16_t NetMessage::unpackInt16()
{
  int16_t v = 0;
  char *p = checkReadBuffer(sizeof(int16_t));
  if (p) {
    nboUnpackInt16(p, v);
    readIndex += sizeof(int16_t);
  }
  return v;
}

int32_t NetMessage::unpackInt32()
{
  int32_t v = 0;
  char *p = checkReadBuffer(sizeof(int32_t));
  if (p) {
    nboUnpackInt32(p, v);
    readIndex += sizeof(int32_t);
  }
  return v;
}

int64_t NetMessage::unpackInt64()
{
  int64_t v = 0;
  char *p = checkReadBuffer(sizeof(int64_t));
  if (p) {
    nboUnpackInt64(p, v);
    readIndex += sizeof(int64_t);
  }
  return v;
}

uint8_t NetMessage::unpackUInt8()
{
  uint8_t v = 0;
  char *p = checkReadBuffer(sizeof(uint8_t));
  if (p) {
    nboUnpackUInt8(p, v);
    readIndex += sizeof(uint8_t);
  }
  return v;
}

uint16_t NetMessage::unpackUInt16()
{
  uint16_t v = 0;
  char *p = checkReadBuffer(sizeof(uint16_t));
  if (p) {
    nboUnpackUInt16(p, v);
    readIndex += sizeof(uint16_t);
  }
  return v;
}

uint32_t NetMessage::unpackUInt32()
{
  uint32_t v = 0;
  char *p = checkReadBuffer(sizeof(uint32_t));
  if (p) {
    nboUnpackUInt32(p, v);
    readIndex += sizeof(uint32_t);
  }
  return v;
}

uint64_t NetMessage::unpackUInt64()
{
  uint64_t v = 0;
  char *p = checkReadBuffer(sizeof(uint64_t));
  if (p) {
    nboUnpackUInt64(p, v);
    readIndex += sizeof(uint64_t);
  }
  return v;
}

float NetMessage::unpackFloat()
{
  float v = 0.0f;
  char *p = checkReadBuffer(sizeof(float));
  if (p) {
    nboUnpackFloat(p, v);
    readIndex += sizeof(float);
  }
  return v;
}

double NetMessage::unpackDouble()
{
  double v = 0.0;
  char *p = checkReadBuffer(sizeof(double));
  if (p) {
    nboUnpackDouble(p, v);
    readIndex += sizeof(double);
  }
  return v;
}

fvec2 NetMessage::unpackFVec2()
{
  fvec2 v(0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec2));
  if (p) {
    nboUnpackFVec2(p, v);
    readIndex += sizeof(fvec2);
  }
  return v;
}

fvec3 NetMessage::unpackFVec3()
{
  fvec3 v(0.0f, 0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec3));
  if (p) {
    nboUnpackFVec3(p, v);
    readIndex += sizeof(fvec3);
  }
  return v;
}

fvec4 NetMessage::unpackFVec4()
{
  fvec4 v(0.0f, 0.0f, 0.0f, 0.0f);
  char *p = checkReadBuffer(sizeof(fvec4));
  if (p) {
    nboUnpackFVec4(p, v);
    readIndex += sizeof(fvec4);
  }
  return v;
}

std::string NetMessage::unpackStdString()
{
  std::string v;
  const uint32_t strSize = unpackUInt32();
  char *p = checkReadBuffer(strSize);
  if (p) {
    char* buffer = new char[strSize + 1];
    nboUnpackString(p, buffer, strSize);
    buffer[strSize] = 0;
    v = buffer;
    delete[] buffer;
    readIndex += strSize;
  }
  return v;
}

std::string NetMessage::unpackStdStringRaw()
{
  std::string v;
  const uint32_t strSize = unpackUInt32();
  char *p = checkReadBuffer(strSize);
  if (p) {
    char* buffer = new char[strSize];
    nboUnpackString(p, buffer, strSize);
    v.assign(buffer, strSize);
    delete[] buffer;
    readIndex += strSize;
  }
  return v;
}

void NetMessage::unpackString(char* buf, size_t len)
{
  char* p = checkReadBuffer(len);
  if (p) {
    memcpy(buf, p, len);
    readIndex += len;
  } else {
    memset(buf, 0, len);
  }
}

//============================================================================//

// Local Variables: ***
// mode: C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<|endoftext|>


<commit_before>// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#include <image_util/geometry_util.h>

#include <cmath>
#include <limits>
#include <vector>

#include <QPolygonF>
#include <QPointF>

#include <opencv2/imgproc/imgproc.hpp>

#include <lapackpp.h>

#include <math_util/constants.h>

#include <ros/ros.h>

namespace image_util
{
  bool Intersects(const BoundingBox& box1, const BoundingBox& box2)
  {
    return (box1 & box2).area() > 0;
  }

  double GetOverlappingArea(const cv::Rect& rect, const cv::Mat& rigid_transform)
  {
    // List of points corresponding to the input rectangle.
    std::vector<cv::Vec2f> points;

    // List of points correspondng to the transformed rectangle.
    std::vector<cv::Vec2f> points_t;

    // Create a point for each corner of the input rectangle.
    points.push_back(cv::Vec2f(rect.x, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y + rect.height));
    points.push_back(cv::Vec2f(rect.x, rect.y + rect.height));

    // Transform the input points to the transformed points using the rigid
    // transform.
    cv::transform(cv::InputArray(points), cv::OutputArray(points_t), rigid_transform);

    // Use the QPolygon object to get the intersecting area of the input
    // rectangle and the transformed rectangle.

    // Build the polygon corresponding to the input rectangle.
    QPolygonF polygon;
    polygon << QPointF(points[0][0], points[0][1]);
    polygon << QPointF(points[1][0], points[1][1]);
    polygon << QPointF(points[2][0], points[2][1]);
    polygon << QPointF(points[3][0], points[3][1]);
    polygon << QPointF(points[0][0], points[0][1]);

    // Build the polygon corresponding to the transformed rectangle.
    QPolygonF transformed_polygon;
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);
    transformed_polygon << QPointF(points_t[1][0], points_t[1][1]);
    transformed_polygon << QPointF(points_t[2][0], points_t[2][1]);
    transformed_polygon << QPointF(points_t[3][0], points_t[3][1]);
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);

    // Get the polygon representing the intersection of the input rectangle and
    // the transformed rectangle.
    QPolygonF intersection = polygon.intersected(transformed_polygon);

    // If the intersection is empty, then just return 0 area.
    if (intersection.size() == 0)
    {
      return 0;
    }

    // Build an OpenCV contour to measure the area of the intersection.
    std::vector<cv::Point2f> contour;
    for (int i = 0; i < intersection.size(); i++)
    {
      contour.push_back(cv::Point2f(intersection[i].x(), intersection[i].y()));
    }

    // Scale the area based on the scale factor to get the correct value.
    return cv::contourArea(contour);
  }

  cv::Mat ProjectEllipsoid(const cv::Mat& ellipsoid)
  {
    cv::Mat ellipse;

    if (ellipsoid.rows == 3 && ellipsoid.cols == 3 && ellipsoid.type() == CV_32FC1)
    {
      if (ellipsoid.at<float>(2, 2) >= std::numeric_limits<double>::max() * 0.5)
      {
        ellipse.create(2, 2, CV_32FC1);
        ellipse.at<float>(0, 0) = ellipsoid.at<float>(0, 0);
        ellipse.at<float>(0, 1) = ellipsoid.at<float>(0, 1);
        ellipse.at<float>(1, 0) = ellipsoid.at<float>(1, 0);
        ellipse.at<float>(1, 1) = ellipsoid.at<float>(1, 1);

        return ellipse;
      }

      LaGenMatDouble A(3, 3);
      for (int32_t r = 0; r < 3; r++)
      {
        for (int32_t c = 0; c < 3; c++)
        {
          A(r, c) = ellipsoid.at<float>(r, c);
        }
      }

      // Specify the main vector directions (x and y)
      LaVectorDouble ax1 = LaVectorDouble::zeros(3, 1);
      LaVectorDouble ax2 = LaVectorDouble::zeros(3, 1);
      ax1(0) = 1;
      ax2(1) = 1;

      // Initialize the normal vector (here the normal vector, in the state
      // space is in the theta direction).
      LaVectorDouble n_ax = LaVectorDouble::zeros(3, 1);
      n_ax(2) = 1;

      ///////////////////////
      // Calculate A prime //
      ///////////////////////

      LaGenMatDouble A_sym_temp = A;
      LaGenMatDouble temp1 = LaGenMatDouble::eye(A.rows(), A.cols());
      Blas_Mat_Trans_Mat_Mult(A, temp1, A_sym_temp, 1.0, 1.0);

      // N_ax_temp = (n_ax*n_ax')
      LaGenMatDouble N_ax_temp = LaGenMatDouble::zeros(3, 3);
      Blas_R1_Update(N_ax_temp, n_ax, n_ax);

      // A_prime_1 = A_sym_temp*N_ax_temp
      //           = (A+A')*(n_ax*n_ax')
      LaGenMatDouble A_prime_1 = LaGenMatDouble::zeros(3, 3);
      Blas_Mat_Mat_Mult(A_sym_temp, N_ax_temp, A_prime_1);

      // A_prime_2 = A_prime_1*A_sym_temp
      //           = (A+A')*(n_ax*n_ax')*(A+A')
      LaGenMatDouble A_prime_2 = LaGenMatDouble::zeros(3, 3);
      Blas_Mat_Mat_Mult(A_prime_1, A_sym_temp, A_prime_2);

      // scale_1 = n_ax'*A
      LaGenMatDouble scale_1(1, 3);
      Blas_Mat_Trans_Mat_Mult(n_ax, A, scale_1);

      // scale_2 = (scale_1*n_ax)*-4
      //         = (n_ax'*A*n_ax)*-4
      LaGenMatDouble scale_2(1, 1);
      Blas_Mat_Mat_Mult(scale_1, n_ax, scale_2);
      scale_2.scale(-4.0);

      // Convert from matrix to scalar
      double scale = scale_2(0, 0);

      // A_temp = scale*A_temp
      //        = scale_2*A = -4*(n_ax'*A*n_ax)*A
      LaGenMatDouble A_temp = A;
      A_temp.scale(scale);

      // Aprime = A_prime_2 + A_temp
      //        = (A+A')*(n_ax*n_ax')*(A+A') - 4*(n_ax'*A*n_ax)*A
      LaGenMatDouble Aprime(3, 3);
      Aprime = A_prime_2 + A_temp;

      ///////////////////////
      // Calculate C prime //
      ///////////////////////

      // C_temp = n_ax'*A
      LaGenMatDouble C_temp(1, 3);
      Blas_Mat_Trans_Mat_Mult(n_ax, A, C_temp);

      // Cprime = -4.0*C_temp*n_ax
      //        = -4.0*n_ax'*A*n_ax
      LaGenMatDouble Cprime(1, 1);
      Blas_Mat_Mat_Mult(C_temp, n_ax, Cprime, -4.0);

      double cp = Cprime(0, 0);

      // Bprime = Aprime/Cprime;
      LaGenMatDouble Bprime = Aprime;
      Bprime.scale(1.0 / cp);

      // Jp = axes_def;
      //    = [ax1(:),ax2(:)] = [1,0;0,1;0,0];
      LaGenMatDouble Jp = LaGenMatDouble::eye(3, 2);

      ///////////////////////
      // Calculate D prime //
      ///////////////////////

      // Dprime_temp = Jp'*Bprime
      LaGenMatDouble Dprime_temp(2, 3);
      Blas_Mat_Trans_Mat_Mult(Jp, Bprime, Dprime_temp);

      // Dprime = Dprime_temp * Jp
      //        = Jp'*Bprime*Jp
      LaGenMatDouble Dprime(2, 2);
      Blas_Mat_Mat_Mult(Dprime_temp, Jp, Dprime);

      for (int32_t r = 0; r < 2; r++)
      {
        for (int32_t c = 0; c < 2; c++)
        {
          if (Dprime(r, c) != Dprime(r, c))
          {
            return ellipse;
          }
        }
      }

      ellipse.create(2, 2, CV_32FC1);
      ellipse.at<float>(0, 0) = Dprime(0, 0);
      ellipse.at<float>(0, 1) = Dprime(0, 1);
      ellipse.at<float>(1, 0) = Dprime(1, 0);
      ellipse.at<float>(1, 1) = Dprime(1, 1);
    }

    return ellipse;
  }

  std::vector<tf::Vector3> GetEllipsePoints(
      const cv::Mat& ellipse,
      const tf::Vector3& center,
      double scale,
      int32_t num_points)
  {
    std::vector<tf::Vector3> perimeter;

    if (ellipse.rows == 2 && ellipse.cols == 2 && ellipse.type() == CV_32FC1 &&
        num_points > 2)
    {
      LaGenMatDouble Dprime(2, 2);
      Dprime(0, 0) = ellipse.at<float>(0, 0);
      Dprime(0, 1) = ellipse.at<float>(0, 1);
      Dprime(1, 0) = ellipse.at<float>(1, 0);
      Dprime(1, 1) = ellipse.at<float>(1, 1);

      LaVectorDouble Sigma(2);
      LaGenMatDouble U(2, 2);
      LaGenMatDouble Vt(2, 2);

      LaSVD_IP(Dprime, Sigma, U, Vt);

      double xprime_scale = sqrt(Sigma(0));
      double yprime_scale = sqrt(Sigma(1));

      if (xprime_scale <= 0 || yprime_scale <= 0)
      {
        return perimeter;
      }

      LaGenMatDouble Xp1 = LaGenMatDouble::zeros(num_points, 2);
      for (int32_t i = 0; i < num_points; i++)
      {
        double phi =
            (static_cast<double>(i) / static_cast<double>(num_points))
            * math_util::_2pi;

        Xp1(i, 0) = xprime_scale * std::cos(phi) * scale;
        Xp1(i, 1) = yprime_scale * std::sin(phi) * scale;
      }

      LaGenMatDouble Xell(num_points, 2);

      // Xell = Xp1*(V')'
      Blas_Mat_Mat_Trans_Mult(Xp1, Vt, Xell);

      perimeter.resize(num_points);
      for (int32_t i = 0; i < num_points; i++)
      {
        perimeter[i].setX(Xell(i, 0) + center.x());
        perimeter[i].setY(Xell(i, 1) + center.y());
      }
    }

    return perimeter;
  }
}
<commit_msg>remove ros header<commit_after>// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#include <image_util/geometry_util.h>

#include <cmath>
#include <limits>
#include <vector>

#include <QPolygonF>
#include <QPointF>

#include <opencv2/imgproc/imgproc.hpp>

#include <lapackpp.h>

#include <math_util/constants.h>

namespace image_util
{
  bool Intersects(const BoundingBox& box1, const BoundingBox& box2)
  {
    return (box1 & box2).area() > 0;
  }

  double GetOverlappingArea(const cv::Rect& rect, const cv::Mat& rigid_transform)
  {
    // List of points corresponding to the input rectangle.
    std::vector<cv::Vec2f> points;

    // List of points correspondng to the transformed rectangle.
    std::vector<cv::Vec2f> points_t;

    // Create a point for each corner of the input rectangle.
    points.push_back(cv::Vec2f(rect.x, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y + rect.height));
    points.push_back(cv::Vec2f(rect.x, rect.y + rect.height));

    // Transform the input points to the transformed points using the rigid
    // transform.
    cv::transform(cv::InputArray(points), cv::OutputArray(points_t), rigid_transform);

    // Use the QPolygon object to get the intersecting area of the input
    // rectangle and the transformed rectangle.

    // Build the polygon corresponding to the input rectangle.
    QPolygonF polygon;
    polygon << QPointF(points[0][0], points[0][1]);
    polygon << QPointF(points[1][0], points[1][1]);
    polygon << QPointF(points[2][0], points[2][1]);
    polygon << QPointF(points[3][0], points[3][1]);
    polygon << QPointF(points[0][0], points[0][1]);

    // Build the polygon corresponding to the transformed rectangle.
    QPolygonF transformed_polygon;
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);
    transformed_polygon << QPointF(points_t[1][0], points_t[1][1]);
    transformed_polygon << QPointF(points_t[2][0], points_t[2][1]);
    transformed_polygon << QPointF(points_t[3][0], points_t[3][1]);
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);

    // Get the polygon representing the intersection of the input rectangle and
    // the transformed rectangle.
    QPolygonF intersection = polygon.intersected(transformed_polygon);

    // If the intersection is empty, then just return 0 area.
    if (intersection.size() == 0)
    {
      return 0;
    }

    // Build an OpenCV contour to measure the area of the intersection.
    std::vector<cv::Point2f> contour;
    for (int i = 0; i < intersection.size(); i++)
    {
      contour.push_back(cv::Point2f(intersection[i].x(), intersection[i].y()));
    }

    // Scale the area based on the scale factor to get the correct value.
    return cv::contourArea(contour);
  }

  cv::Mat ProjectEllipsoid(const cv::Mat& ellipsoid)
  {
    cv::Mat ellipse;

    if (ellipsoid.rows == 3 && ellipsoid.cols == 3 && ellipsoid.type() == CV_32FC1)
    {
      if (ellipsoid.at<float>(2, 2) >= std::numeric_limits<double>::max() * 0.5)
      {
        ellipse.create(2, 2, CV_32FC1);
        ellipse.at<float>(0, 0) = ellipsoid.at<float>(0, 0);
        ellipse.at<float>(0, 1) = ellipsoid.at<float>(0, 1);
        ellipse.at<float>(1, 0) = ellipsoid.at<float>(1, 0);
        ellipse.at<float>(1, 1) = ellipsoid.at<float>(1, 1);

        return ellipse;
      }

      LaGenMatDouble A(3, 3);
      for (int32_t r = 0; r < 3; r++)
      {
        for (int32_t c = 0; c < 3; c++)
        {
          A(r, c) = ellipsoid.at<float>(r, c);
        }
      }

      // Specify the main vector directions (x and y)
      LaVectorDouble ax1 = LaVectorDouble::zeros(3, 1);
      LaVectorDouble ax2 = LaVectorDouble::zeros(3, 1);
      ax1(0) = 1;
      ax2(1) = 1;

      // Initialize the normal vector (here the normal vector, in the state
      // space is in the theta direction).
      LaVectorDouble n_ax = LaVectorDouble::zeros(3, 1);
      n_ax(2) = 1;

      ///////////////////////
      // Calculate A prime //
      ///////////////////////

      LaGenMatDouble A_sym_temp = A;
      LaGenMatDouble temp1 = LaGenMatDouble::eye(A.rows(), A.cols());
      Blas_Mat_Trans_Mat_Mult(A, temp1, A_sym_temp, 1.0, 1.0);

      // N_ax_temp = (n_ax*n_ax')
      LaGenMatDouble N_ax_temp = LaGenMatDouble::zeros(3, 3);
      Blas_R1_Update(N_ax_temp, n_ax, n_ax);

      // A_prime_1 = A_sym_temp*N_ax_temp
      //           = (A+A')*(n_ax*n_ax')
      LaGenMatDouble A_prime_1 = LaGenMatDouble::zeros(3, 3);
      Blas_Mat_Mat_Mult(A_sym_temp, N_ax_temp, A_prime_1);

      // A_prime_2 = A_prime_1*A_sym_temp
      //           = (A+A')*(n_ax*n_ax')*(A+A')
      LaGenMatDouble A_prime_2 = LaGenMatDouble::zeros(3, 3);
      Blas_Mat_Mat_Mult(A_prime_1, A_sym_temp, A_prime_2);

      // scale_1 = n_ax'*A
      LaGenMatDouble scale_1(1, 3);
      Blas_Mat_Trans_Mat_Mult(n_ax, A, scale_1);

      // scale_2 = (scale_1*n_ax)*-4
      //         = (n_ax'*A*n_ax)*-4
      LaGenMatDouble scale_2(1, 1);
      Blas_Mat_Mat_Mult(scale_1, n_ax, scale_2);
      scale_2.scale(-4.0);

      // Convert from matrix to scalar
      double scale = scale_2(0, 0);

      // A_temp = scale*A_temp
      //        = scale_2*A = -4*(n_ax'*A*n_ax)*A
      LaGenMatDouble A_temp = A;
      A_temp.scale(scale);

      // Aprime = A_prime_2 + A_temp
      //        = (A+A')*(n_ax*n_ax')*(A+A') - 4*(n_ax'*A*n_ax)*A
      LaGenMatDouble Aprime(3, 3);
      Aprime = A_prime_2 + A_temp;

      ///////////////////////
      // Calculate C prime //
      ///////////////////////

      // C_temp = n_ax'*A
      LaGenMatDouble C_temp(1, 3);
      Blas_Mat_Trans_Mat_Mult(n_ax, A, C_temp);

      // Cprime = -4.0*C_temp*n_ax
      //        = -4.0*n_ax'*A*n_ax
      LaGenMatDouble Cprime(1, 1);
      Blas_Mat_Mat_Mult(C_temp, n_ax, Cprime, -4.0);

      double cp = Cprime(0, 0);

      // Bprime = Aprime/Cprime;
      LaGenMatDouble Bprime = Aprime;
      Bprime.scale(1.0 / cp);

      // Jp = axes_def;
      //    = [ax1(:),ax2(:)] = [1,0;0,1;0,0];
      LaGenMatDouble Jp = LaGenMatDouble::eye(3, 2);

      ///////////////////////
      // Calculate D prime //
      ///////////////////////

      // Dprime_temp = Jp'*Bprime
      LaGenMatDouble Dprime_temp(2, 3);
      Blas_Mat_Trans_Mat_Mult(Jp, Bprime, Dprime_temp);

      // Dprime = Dprime_temp * Jp
      //        = Jp'*Bprime*Jp
      LaGenMatDouble Dprime(2, 2);
      Blas_Mat_Mat_Mult(Dprime_temp, Jp, Dprime);

      for (int32_t r = 0; r < 2; r++)
      {
        for (int32_t c = 0; c < 2; c++)
        {
          if (Dprime(r, c) != Dprime(r, c))
          {
            return ellipse;
          }
        }
      }

      ellipse.create(2, 2, CV_32FC1);
      ellipse.at<float>(0, 0) = Dprime(0, 0);
      ellipse.at<float>(0, 1) = Dprime(0, 1);
      ellipse.at<float>(1, 0) = Dprime(1, 0);
      ellipse.at<float>(1, 1) = Dprime(1, 1);
    }

    return ellipse;
  }

  std::vector<tf::Vector3> GetEllipsePoints(
      const cv::Mat& ellipse,
      const tf::Vector3& center,
      double scale,
      int32_t num_points)
  {
    std::vector<tf::Vector3> perimeter;

    if (ellipse.rows == 2 && ellipse.cols == 2 && ellipse.type() == CV_32FC1 &&
        num_points > 2)
    {
      LaGenMatDouble Dprime(2, 2);
      Dprime(0, 0) = ellipse.at<float>(0, 0);
      Dprime(0, 1) = ellipse.at<float>(0, 1);
      Dprime(1, 0) = ellipse.at<float>(1, 0);
      Dprime(1, 1) = ellipse.at<float>(1, 1);

      LaVectorDouble Sigma(2);
      LaGenMatDouble U(2, 2);
      LaGenMatDouble Vt(2, 2);

      LaSVD_IP(Dprime, Sigma, U, Vt);

      double xprime_scale = sqrt(Sigma(0));
      double yprime_scale = sqrt(Sigma(1));

      if (xprime_scale <= 0 || yprime_scale <= 0)
      {
        return perimeter;
      }

      LaGenMatDouble Xp1 = LaGenMatDouble::zeros(num_points, 2);
      for (int32_t i = 0; i < num_points; i++)
      {
        double phi =
            (static_cast<double>(i) / static_cast<double>(num_points))
            * math_util::_2pi;

        Xp1(i, 0) = xprime_scale * std::cos(phi) * scale;
        Xp1(i, 1) = yprime_scale * std::sin(phi) * scale;
      }

      LaGenMatDouble Xell(num_points, 2);

      // Xell = Xp1*(V')'
      Blas_Mat_Mat_Trans_Mult(Xp1, Vt, Xell);

      perimeter.resize(num_points);
      for (int32_t i = 0; i < num_points; i++)
      {
        perimeter[i].setX(Xell(i, 0) + center.x());
        perimeter[i].setY(Xell(i, 1) + center.y());
      }
    }

    return perimeter;
  }
}
<|endoftext|>


<commit_before>#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <cassert>

struct Container {
  uint8_t* data_;
  uint8_t* current_;
  int size_;
  int ref_count_;

  Container(int size)
    : size_(size), ref_count_(1) {
    data_ = reinterpret_cast<uint8_t*>(calloc(size_, 1));
    current_ = data_;
  }

  ~Container() {
    assert(ref_count_ == 0);
    free(data_);
  }

  void* Place(int size) {
    if (current_ + size > data_ + size_) {
      return nullptr;
    }
    void* result = current_;
    current_ += size;
    return result;
  }

  void AddRef() {
    if (data_) {
      ref_count_++;
    }
  }

  void Release() {
    if (data_) {
      ref_count_--;
    }
  }

  void Dispose() {
    // Destroy container ignoring non-zero refcount. Use with care.
    ref_count_ = 0;
    free(data_);
    data_ = nullptr;
    current_ = nullptr;
  }
};

template <class T>
struct Ref {
  T* ref_;
  Container* container_;

  Ref() : ref_(nullptr), container_(nullptr) {}
  Ref(const Ref& other) : ref_(nullptr), container_(nullptr) {
    Assign(other);
  }
  Ref(T* ref, Container* container) : ref_(ref), container_(container) {
    if (container_) {
      container_->AddRef();
    }
  }
  ~Ref() {
    if (ref_ && container_) {
      container_->Release();
    }
  }

  static Ref<T> Alloc(Container* container) {
    return Ref<T>(reinterpret_cast<T*>(container->Place(sizeof(T))), container);
  }

  void Assign(const Ref<T>& other) {
    if (ref_) {
      container_->Release();
    }
    container_ = other.container_;
    container_->AddRef();
    ref_ = other.ref_;
  }

   T* operator->() const {
     return ref_;
   }

  bool null() const { return ref_ == nullptr; }
};

struct List {
  Ref<List> next_;
  int data_;
};

void test_placer() {
  printf("Start placement\n");
  Container heap(1024);

  Ref<List> head = Ref<List>::Alloc(&heap);
  head->data_ = 1;
  Ref<List> cur = head;
  for (int i = 0; i < 10; ++i) {
    cur->next_ = Ref<List>::Alloc(&heap);
    cur = cur->next_;
    cur->data_ = i + 2;
  }
  cur = head;
  while (!cur.null()) {
    printf("next is %d\n", cur->data_);
    cur = cur->next_;
  }

  heap.Dispose();
}

int main() {
  test_placer();
}
<commit_msg>Reference is single value.<commit_after>#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <cassert>

class Container {
 private:
  uint8_t* data_;
  uint8_t* current_;
  int size_;
  int ref_count_;

 public:
  Container(int size)
    : size_(size), ref_count_(1) {
    data_ = reinterpret_cast<uint8_t*>(calloc(size_, 1));
    current_ = data_;
  }

  ~Container() {
    assert(ref_count_ == 0);
    free(data_);
  }

  void* Place(int size) {
    size += sizeof(Container*);
    if (current_ + size > data_ + size_) {
      return nullptr;
    }
    Container** result = reinterpret_cast<Container**>(current_);
    *result = this;
    current_ += size;
    return result;
  }

  void AddRef() {
    if (data_) {
      //printf("addref %d\n", ref_count_);
      ref_count_++;
    }
  }

  void Release() {
    if (data_) {
      // printf("release %d\n", ref_count_);
      ref_count_--;
    }
  }

  void Dispose() {
    // Destroy container ignoring non-zero refcount. Use with care.
    ref_count_ = 0;
    free(data_);
    data_ = nullptr;
    current_ = nullptr;
  }
};

template <class T>
class Ref {
 private:
  void* ptr_;

  explicit Ref(void* ptr) : ptr_(ptr) {
    if (ptr_) {
      container()->AddRef();
    }
  }

 public:
  Ref() : ptr_(nullptr) {}
  Ref(const Ref& other) : ptr_(nullptr) {
    Assign(other);
  }
  Ref& operator=(const Ref& other) {
    Assign(other);
    return *this;
  }

  ~Ref() {
    if (ptr_) {
      container()->Release();
    }
  }

  T* ref() const {
    if (!ptr_) return nullptr;
    return reinterpret_cast<T*>(reinterpret_cast<uint8_t*>(ptr_) + sizeof(Container*));
  }

  Container* container() {
    return *reinterpret_cast<Container**>(ptr_);
  }

  void Assign(const Ref<T>& other) {
    // TODO: optimize for an important case where containers matches.
    if (ptr_) {
      container()->Release();
    }
    ptr_ = other.ptr_;
    if (ptr_) {
      container()->AddRef();
    }
  }

  T* operator->() const {
    return ref();
  }

  bool null() const { return ptr_ == nullptr; }

  static Ref<T> Alloc(Container* container) {
    return Ref<T>(container->Place(sizeof(T)));
  }
};

struct List {
  Ref<List> next_;
  int data_;
};

void test_placer() {
  printf("Start placement\n");
  Container heap(1024);
  {
    Ref<List> head = Ref<List>::Alloc(&heap);
    head->data_ = 1;
    Ref<List> cur = head;
    for (int i = 0; i < 10; ++i) {
      cur->next_ = Ref<List>::Alloc(&heap);
      cur = cur->next_;
      cur->data_ = i + 2;
    }
    cur = head;
    while (!cur.null()) {
      printf("next is %d\n", cur->data_);
      cur = cur->next_;
    }
  }
  heap.Dispose();
}

int main() {
  test_placer();
  return 0;
}
<|endoftext|>


<commit_before>#include "util.h"
#include "common.h"

int error(const char *errs, ...) {
  va_list args;
  va_start(args, errs);
    vprintf(errs, args); puts("");
  va_end(args);
  exit(0);
  return 0;
}

char *replace_escape(char *str) {
  int i;
  char *pos;
  char escape[14][3] = {
    "\\a", "\a",
    "\\r", "\r",
    "\\f", "\f",
    "\\n", "\n",
    "\\t", "\t",
    "\\b", "\b",
    "\\\"",  "\""
  };
  for(i = 0; i < 14; i += 2) {
    while((pos = strstr(str, escape[i])) != NULL) {
      *pos = escape[i + 1][0];
      memmove(pos + 1, pos + 2, strlen(str) - 2 + 1);
    }
  }
  return str;
}
<commit_msg>efficiency of replace escape<commit_after>#include "util.h"
#include "common.h"

int error(const char *errs, ...) {
  va_list args;
  va_start(args, errs);
    vprintf(errs, args); puts("");
  va_end(args);
  exit(0);
  return 0;
}

char *replace_escape(char *str) {
  int l = strlen(str);
  for(int i = 0; i < l; i++) {
    if(str[i] == '\\') {
      i++; switch(str[i]) {
        case 'a': str[i]='\a'; break;
        case 'n': str[i]='\n'; break;
        case 't': str[i]='\t'; break;
        case 'f': str[i]='\f'; break;
        case 'r': str[i]='\r'; break;
        case 'b': str[i]='\b'; break;
        case '\\':str[i]='\\'; break;
        case '\'':str[i]='\''; break;
        case '\"':str[i]='\"'; break;
      }
      memmove(&str[i-1], &str[i], l - 2 + 1);
    }
  }
  return str;
}
<|endoftext|>