<commit_before>// Copyright (c) 2013, Kirk Shoop (kirk.shoop@gmail.com)
// 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.
//
// 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
//
#ifndef WIN32_WINDOW_NAMESPACE
#error WIN32_WINDOW_NAMESPACE must be defined
#endif

namespace WIN32_WINDOW_NAMESPACE
{

	namespace detail
	{

		template<typename T>
		struct raw_ptr
		{
			explicit raw_ptr(T* raw) : raw(raw) {}
			operator typename cmn::unspecified_bool<raw_ptr>::type () { return cmn::unspecified_bool<raw_ptr>::get(!!raw); }
			T* operator ->() { return raw; }
			T* raw;
		};
	}

	template<typename WindowClassTag>
	//static
	ATOM window_class<WindowClassTag>::Register()
	{
		WNDCLASSEX wcex = {};
		wcex.cbSize = sizeof(WNDCLASSEX);

		// defaults that can be overriden
		wcex.style = CS_HREDRAW | CS_VREDRAW;
		wcex.hInstance = GetCurrentInstance();
		wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);

		window_class_register(&wcex, tag());

		// not overridable
		wcex.lpfnWndProc = WindowCallback<WindowClassTag>;

		return RegisterClassEx(&wcex);
	}

	template<typename WindowClassTag>
	template<typename T>
	//static
	ATOM window_class<WindowClassTag>::Register(T&& t)
	{
		WNDCLASSEX wcex = {};
		wcex.cbSize = sizeof(WNDCLASSEX);

		// defaults that can be overriden
		wcex.style = CS_HREDRAW | CS_VREDRAW;
		wcex.hInstance = cmn::GetCurrentInstance();
		wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);

		window_class_register(std::forward<T>(t), &wcex, tag());

		// not overridable
		wcex.lpfnWndProc = WindowCallback<WindowClassTag>;

		return RegisterClassEx(&wcex);
	}

	template<typename WindowClassTag>
	struct Context
	{
		HWND window;
		UINT message;
		WPARAM wParam;
		LPARAM lParam;
	};

 	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_constructor(HWND hwnd, LPCREATESTRUCT createStruct, decltype(new (std::nothrow) window_class<WindowClassTag>::traits::type(cmn::instance_of<HWND>::value, cmn::instance_of<CREATESTRUCT>::value)))
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		Type;

		std::unique_ptr<Type> type(new (std::nothrow) Type(hwnd, *createStruct));

		return std::move(type);
	}

	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_constructor(HWND , LPCREATESTRUCT , ...)
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		Type;

		std::unique_ptr<Type> type(new (std::nothrow) Type);

		return std::move(type);
	}

	template<typename WindowClassTag>
	decltype(
		window_class_construct(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<LPCREATESTRUCT>::value,
			WindowClassTag()))
	optional_window_class_construct(HWND hwnd, LPCREATESTRUCT createStruct, WindowClassTag&&, int)
	{
		return window_class_construct(hwnd, createStruct, WindowClassTag());
	}

 	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_construct(HWND hwnd, LPCREATESTRUCT createStruct, WindowClassTag&&, ...)
	{
		return optional_window_class_constructor<WindowClassTag>(hwnd, createStruct, 0);
	}

	template<typename WindowClassTag, typename T>
	decltype(
		window_class_insert(
			cmn::instance_of<HWND>::value,
			WindowClassTag()))
	optional_window_class_insert(HWND hwnd, T t, WindowClassTag&&, int)
	{
		return window_class_insert(hwnd, std::move(t), WindowClassTag());
	}

 	template<typename WindowClassTag>
	bool optional_window_class_insert(
		HWND hwnd,
		std::unique_ptr<typename window_class<WindowClassTag>::traits::type> type,
		WindowClassTag&&,
		...)
	{
		if (!type)
		{
			return false;
		}

		SetLastError(0);

		auto result = SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(type.get()));
		unique_winerror winerror = make_winerror_if(!result);

		ON_UNWIND(UserDataUnwind, [&]{SetWindowLongPtr(hwnd, GWLP_USERDATA, 0);});

		if (!winerror)
		{
			return false;
		}

		if (!!result)
		{
			return false;
		}

		type.release();
		UserDataUnwind.dismiss();
		return true;
	}

 	template<typename WindowClassTag>
	decltype(
		window_class_find(
			cmn::instance_of<HWND>::value,
			WindowClassTag()))
	optional_window_class_find(HWND hwnd, WindowClassTag&&, int)
	{
		return window_class_find(hwnd, WindowClassTag());
	}

 	template<typename WindowClassTag>
	detail::raw_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_find(HWND hwnd, WindowClassTag&&, ...)
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		type;
		return detail::raw_ptr<type>(reinterpret_cast<type*>(GetWindowLongPtr(hwnd, GWLP_USERDATA)));
	}

 	template<typename Type, typename WindowClassTag>
	decltype(
		window_class_erase(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<Type>::value,
			WindowClassTag()))
	optional_window_class_erase(HWND hwnd, Type type, WindowClassTag&&, int)
	{
		return window_class_erase(hwnd, type, WindowClassTag());
	}

 	template<typename Type, typename WindowClassTag>
	void optional_window_class_erase(HWND hwnd, detail::raw_ptr<Type> , WindowClassTag&&, ...)
	{
		SetWindowLongPtr(hwnd, GWLP_USERDATA, 0);
	}

	template<typename Type, typename WindowClassTag>
	decltype(
		window_class_destroy(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<Type>::value,
			WindowClassTag()))
	optional_window_class_destroy(HWND hwnd, Type type, WindowClassTag&&, int)
	{
		return window_class_destroy(hwnd, type, WindowClassTag());
	}

 	template<typename Type, typename WindowClassTag>
	void optional_window_class_destroy(HWND , detail::raw_ptr<Type> type, WindowClassTag&&, ...)
	{
		delete type.raw;
	}

    template<typename Type, typename WindowClassTag>
	decltype(
		window_class_dispatch(
			cmn::instance_of<Type>::value,
            cmn::instance_of<Context<WindowClassTag>>::value,
			WindowClassTag()))
	optional_window_class_dispatch(Type type, const Context<WindowClassTag>& context, WindowClassTag&&, int)
	{
        return window_message_error_contract(
            [type] (const Context<WindowClassTag>& context)
            {
                return window_class_dispatch(type, context, WindowClassTag());
            },
            context,
            WindowClassTag()
        );
	}

    typedef std::pair<bool, LRESULT> dispatch_result;

    template<typename Type, typename WindowClassTag>
	dispatch_result optional_window_class_dispatch(detail::raw_ptr<Type> type, const Context<WindowClassTag>& context, WindowClassTag&&, ...)
	{
        return window_message_error_contract(
            [type] (const Context<WindowClassTag>& context)
            {
                return type->window_proc(context.window, context.message, context.wParam, context.LParam);
            },
            context,
            WindowClassTag()
        );
	}

	namespace detail
	{
		template<typename WindowClassTag>
		LRESULT CALLBACK WindowCallbackSafe(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
		{
			bool handled = false;
			LRESULT result = 0;

			if (message == WM_NCCREATE)
			{
				auto existingType = optional_window_class_find(hWnd, WindowClassTag(), 0);
				if  (!existingType)
				{
					if (!optional_window_class_insert(
							hWnd,
							optional_window_class_construct(
								hWnd,
								reinterpret_cast<LPCREATESTRUCT>(lParam),
								WindowClassTag(),
								0
							),
							WindowClassTag(),
							0
						)
					)
					{
						return FALSE;
					}
				}
			}

			auto type = optional_window_class_find(hWnd, WindowClassTag(), 0);

			ON_UNWIND_AUTO(
				[&]
				{
					if (type && message == WM_NCDESTROY)
					{
						optional_window_class_erase(hWnd, type, WindowClassTag(), 0);
						optional_window_class_destroy(hWnd, type, WindowClassTag(), 0);
					}
				}
			);

			if (type)
			{
				Context<WindowClassTag> context = {hWnd, message, wParam, lParam};

				std::tie(handled, result) = optional_window_class_dispatch(type, context, WindowClassTag(), 0);
				if (handled)
				{
					return result;
				}
			}

			return DefWindowProc(hWnd, message, wParam, lParam);
		}
	}

	template<typename WindowClassTag>
	LRESULT CALLBACK WindowCallback(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
	{
		__try
		{
			return detail::WindowCallbackSafe<WindowClassTag>(hWnd, message, wParam, lParam);
		}
		__except(cmn::FailFastFilter(GetExceptionInformation()))
		{
		}
		return 0;
	}

}
<commit_msg>abort window creation if the storage is not empty<commit_after>// Copyright (c) 2013, Kirk Shoop (kirk.shoop@gmail.com)
// 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.
//
// 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
//
#ifndef WIN32_WINDOW_NAMESPACE
#error WIN32_WINDOW_NAMESPACE must be defined
#endif

namespace WIN32_WINDOW_NAMESPACE
{

	namespace detail
	{

		template<typename T>
		struct raw_ptr
		{
			explicit raw_ptr(T* raw) : raw(raw) {}
			operator typename cmn::unspecified_bool<raw_ptr>::type () { return cmn::unspecified_bool<raw_ptr>::get(!!raw); }
			T* operator ->() { return raw; }
			T* raw;
		};
	}

	template<typename WindowClassTag>
	//static
	ATOM window_class<WindowClassTag>::Register()
	{
		WNDCLASSEX wcex = {};
		wcex.cbSize = sizeof(WNDCLASSEX);

		// defaults that can be overriden
		wcex.style = CS_HREDRAW | CS_VREDRAW;
		wcex.hInstance = GetCurrentInstance();
		wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);

		window_class_register(&wcex, tag());

		// not overridable
		wcex.lpfnWndProc = WindowCallback<WindowClassTag>;

		return RegisterClassEx(&wcex);
	}

	template<typename WindowClassTag>
	template<typename T>
	//static
	ATOM window_class<WindowClassTag>::Register(T&& t)
	{
		WNDCLASSEX wcex = {};
		wcex.cbSize = sizeof(WNDCLASSEX);

		// defaults that can be overriden
		wcex.style = CS_HREDRAW | CS_VREDRAW;
		wcex.hInstance = cmn::GetCurrentInstance();
		wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);

		window_class_register(std::forward<T>(t), &wcex, tag());

		// not overridable
		wcex.lpfnWndProc = WindowCallback<WindowClassTag>;

		return RegisterClassEx(&wcex);
	}

	template<typename WindowClassTag>
	struct Context
	{
		HWND window;
		UINT message;
		WPARAM wParam;
		LPARAM lParam;
	};

 	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_constructor(HWND hwnd, LPCREATESTRUCT createStruct, decltype(new (std::nothrow) window_class<WindowClassTag>::traits::type(cmn::instance_of<HWND>::value, cmn::instance_of<CREATESTRUCT>::value)))
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		Type;

		std::unique_ptr<Type> type(new (std::nothrow) Type(hwnd, *createStruct));

		return std::move(type);
	}

	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_constructor(HWND , LPCREATESTRUCT , ...)
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		Type;

		std::unique_ptr<Type> type(new (std::nothrow) Type);

		return std::move(type);
	}

	template<typename WindowClassTag>
	decltype(
		window_class_construct(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<LPCREATESTRUCT>::value,
			WindowClassTag()))
	optional_window_class_construct(HWND hwnd, LPCREATESTRUCT createStruct, WindowClassTag&&, int)
	{
		return window_class_construct(hwnd, createStruct, WindowClassTag());
	}

 	template<typename WindowClassTag>
	std::unique_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_construct(HWND hwnd, LPCREATESTRUCT createStruct, WindowClassTag&&, ...)
	{
		return optional_window_class_constructor<WindowClassTag>(hwnd, createStruct, 0);
	}

	template<typename WindowClassTag, typename T>
	decltype(
		window_class_insert(
			cmn::instance_of<HWND>::value,
			WindowClassTag()))
	optional_window_class_insert(HWND hwnd, T t, WindowClassTag&&, int)
	{
		return window_class_insert(hwnd, std::move(t), WindowClassTag());
	}

 	template<typename WindowClassTag>
	bool optional_window_class_insert(
		HWND hwnd,
		std::unique_ptr<typename window_class<WindowClassTag>::traits::type> type,
		WindowClassTag&&,
		...)
	{
		if (!type)
		{
			return false;
		}

		SetLastError(0);

		auto result = SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(type.get()));
		unique_winerror winerror = make_winerror_if(!result);

		ON_UNWIND(UserDataUnwind, [&]{SetWindowLongPtr(hwnd, GWLP_USERDATA, 0);});

		if (!winerror)
		{
			return false;
		}

		if (!!result)
		{
			return false;
		}

		type.release();
		UserDataUnwind.dismiss();
		return true;
	}

 	template<typename WindowClassTag>
	decltype(
		window_class_find(
			cmn::instance_of<HWND>::value,
			WindowClassTag()))
	optional_window_class_find(HWND hwnd, WindowClassTag&&, int)
	{
		return window_class_find(hwnd, WindowClassTag());
	}

 	template<typename WindowClassTag>
	detail::raw_ptr<typename window_class<WindowClassTag>::traits::type>
	optional_window_class_find(HWND hwnd, WindowClassTag&&, ...)
	{
		typedef
			typename window_class<WindowClassTag>::traits::type
		type;
		return detail::raw_ptr<type>(reinterpret_cast<type*>(GetWindowLongPtr(hwnd, GWLP_USERDATA)));
	}

 	template<typename Type, typename WindowClassTag>
	decltype(
		window_class_erase(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<Type>::value,
			WindowClassTag()))
	optional_window_class_erase(HWND hwnd, Type type, WindowClassTag&&, int)
	{
		return window_class_erase(hwnd, type, WindowClassTag());
	}

 	template<typename Type, typename WindowClassTag>
	void optional_window_class_erase(HWND hwnd, detail::raw_ptr<Type> , WindowClassTag&&, ...)
	{
		SetWindowLongPtr(hwnd, GWLP_USERDATA, 0);
	}

	template<typename Type, typename WindowClassTag>
	decltype(
		window_class_destroy(
			cmn::instance_of<HWND>::value,
			cmn::instance_of<Type>::value,
			WindowClassTag()))
	optional_window_class_destroy(HWND hwnd, Type type, WindowClassTag&&, int)
	{
		return window_class_destroy(hwnd, type, WindowClassTag());
	}

 	template<typename Type, typename WindowClassTag>
	void optional_window_class_destroy(HWND , detail::raw_ptr<Type> type, WindowClassTag&&, ...)
	{
		delete type.raw;
	}

    template<typename Type, typename WindowClassTag>
	decltype(
		window_class_dispatch(
			cmn::instance_of<Type>::value,
            cmn::instance_of<Context<WindowClassTag>>::value,
			WindowClassTag()))
	optional_window_class_dispatch(Type type, const Context<WindowClassTag>& context, WindowClassTag&&, int)
	{
        return window_message_error_contract(
            [type] (const Context<WindowClassTag>& context)
            {
                return window_class_dispatch(type, context, WindowClassTag());
            },
            context,
            WindowClassTag()
        );
	}

    typedef std::pair<bool, LRESULT> dispatch_result;

    template<typename Type, typename WindowClassTag>
	dispatch_result optional_window_class_dispatch(detail::raw_ptr<Type> type, const Context<WindowClassTag>& context, WindowClassTag&&, ...)
	{
        return window_message_error_contract(
            [type] (const Context<WindowClassTag>& context)
            {
                return type->window_proc(context.window, context.message, context.wParam, context.LParam);
            },
            context,
            WindowClassTag()
        );
	}

	namespace detail
	{
		template<typename WindowClassTag>
		LRESULT CALLBACK WindowCallbackSafe(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
		{
			bool handled = false;
			LRESULT result = 0;

			if (message == WM_NCCREATE)
			{
				auto existingType = optional_window_class_find(hWnd, WindowClassTag(), 0);
				if  (existingType)
				{
                    // the slot where we would store our type instance is full. abort.
                    return FALSE;
                }
				if (!optional_window_class_insert(
						hWnd,
						optional_window_class_construct(
							hWnd,
							reinterpret_cast<LPCREATESTRUCT>(lParam),
							WindowClassTag(),
							0
						),
						WindowClassTag(),
						0
					)
				)
				{
					return FALSE;
				}
			}

			auto type = optional_window_class_find(hWnd, WindowClassTag(), 0);

			ON_UNWIND_AUTO(
				[&]
				{
					if (type && message == WM_NCDESTROY)
					{
						optional_window_class_erase(hWnd, type, WindowClassTag(), 0);
						optional_window_class_destroy(hWnd, type, WindowClassTag(), 0);
					}
				}
			);

			if (type)
			{
				Context<WindowClassTag> context = {hWnd, message, wParam, lParam};

				std::tie(handled, result) = optional_window_class_dispatch(type, context, WindowClassTag(), 0);
				if (handled)
				{
					return result;
				}
			}

			return DefWindowProc(hWnd, message, wParam, lParam);
		}
	}

	template<typename WindowClassTag>
	LRESULT CALLBACK WindowCallback(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
	{
		__try
		{
			return detail::WindowCallbackSafe<WindowClassTag>(hWnd, message, wParam, lParam);
		}
		__except(cmn::FailFastFilter(GetExceptionInformation()))
		{
		}
		return 0;
	}

}
<|endoftext|>


<commit_before>// @(#)root/gui:$Name:  $:$Id: TGPicture.cxx,v 1.10 2004/07/09 12:47:02 brun Exp $
// Author: Fons Rademakers   01/01/98

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/
/**************************************************************************

    This source is based on Xclass95, a Win95-looking GUI toolkit.
    Copyright (C) 1996, 1997 David Barth, Ricky Ralston, Hector Peraza.

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

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

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TGPicture & TGPicturePool                                            //
//                                                                      //
// The TGPicture class implements pictures and icons used in the        //
// different GUI elements and widgets. The TGPicturePool class          //
// implements a TGPicture cache. TGPictures are created, managed and    //
// destroyed by the TGPicturePool.                                      //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "TGPicture.h"
#include "TGResourcePool.h"
#include "THashTable.h"
#include "TSystem.h"
#include "TGWindow.h"
#include "TVirtualX.h"

TGGC *TGSelectedPicture::fgSelectedGC = 0;

ClassImp(TGPicture)
ClassImp(TGSelectedPicture)
ClassImp(TGPicturePool)

//______________________________________________________________________________
const TGPicture *TGPicturePool::GetPicture(const char *name)
{
   // Get a picture from the picture pool. Picture must be freed using
   // TGPicturePool::FreePicture(). If picture is not found 0 is returned.

   if (!fPicList)
      fPicList = new THashTable(50);

   TString pname = name;

   if (pname.EndsWith(".xpm")) {
      const char *pxname = gSystem->ExpandPathName(gSystem->UnixPathName(pname));
      pname = pxname;
      delete [] pxname;
   }
   TGPicture *pic;

   pic = (TGPicture *)fPicList->FindObject(pname);
   if (pic && !pic->IsScaled()) {
      if (pic->fPic == kNone)
         return 0;
      pic->AddReference();
      return pic;
   }

   pic = new TGPicture(pname);
   pic->fAttributes.fColormap  = fClient->GetDefaultColormap();
   pic->fAttributes.fCloseness = 40000; // Allow for "similar" colors
   pic->fAttributes.fMask      = kPASize | kPAColormap | kPACloseness;

   char *picnam = gSystem->Which(fPath, pname, kReadPermission);
   if (!picnam) {
      fPicList->Add(pic);
      return 0;
   }

   if (gVirtualX->CreatePictureFromFile(fClient->GetDefaultRoot()->GetId(), picnam,
                                        pic->fPic, pic->fMask,
                                        pic->fAttributes)) {
      fPicList->Add(pic);
   } else {
      delete pic;
      pic = new TGPicture(name);
      fPicList->Add(pic);
      pic = 0;
   }

   delete [] picnam;

   return pic;
}

//______________________________________________________________________________
const TGPicture *TGPicturePool::GetPicture(const char *name,
                                           UInt_t new_width, UInt_t new_height)
{
   // Get picture with specified size from pool (picture will be scaled if
   // necessary). Picture must be freed using TGPicturePool::FreePicture(). If
   // picture is not found 0 is returned.

   if (!fPicList)
      fPicList = new THashTable(50);

   TString pname = name;

   if (pname.EndsWith(".xpm")) {
      const char *pxname = gSystem->ExpandPathName(gSystem->UnixPathName(pname));
      pname = pxname;
      delete [] pxname;
   }

   TGPicture *pic;

   const char *hname = TGPicture::HashName(pname, new_width, new_height);
   pic = (TGPicture *)fPicList->FindObject(hname);
   if (pic && pic->GetWidth() == new_width && pic->GetHeight() == new_height) {
      if (pic->fPic == kNone)
         return 0;
      pic->AddReference();
      return pic;
   }

   pic = new TGPicture(hname, kTRUE);
   pic->fAttributes.fColormap  = fClient->GetDefaultColormap();
   pic->fAttributes.fCloseness = 40000; // Allow for "similar" colors
   pic->fAttributes.fMask      = kPASize | kPAColormap | kPACloseness;

   char *picnam = gSystem->Which(fPath, pname, kReadPermission);
   if (!picnam) {
      pic->fAttributes.fWidth  = new_width;
      pic->fAttributes.fHeight = new_height;
      fPicList->Add(pic);
      return 0;
   }

   Bool_t retc = kFALSE;
   if (!gVirtualX->InheritsFrom("TGX11")) {
      // case of win32gdk and qt drivers
      retc = gVirtualX->CreatePictureFromFile(fClient->GetDefaultRoot()->GetId(),
                                              picnam, pic->fPic, pic->fMask,
                                              pic->fAttributes);
      delete [] picnam;
   } else {
      // case supported by X11 driver
      char **data;
      if (!gVirtualX->ReadPictureDataFromFile(picnam, &data)) {
         delete pic;
         delete [] picnam;
         return 0;
      }
      delete [] picnam;

      Int_t    colors, chars, headersize, totalheight;
      UInt_t   width, height;
      Double_t xscale, yscale;

      sscanf(data[0], "%u %u %d %d", &width, &height, &colors, &chars);
      headersize = colors + 1;
      yscale = (Double_t) new_height / (Double_t) height;
      xscale = (Double_t) new_width / (Double_t) width;
      totalheight = (colors + new_height + 1);

      if ((width != new_width) || (height != new_height)) {
         char **smalldata;
         Int_t    i, x1, y1, pixels;
         Double_t x, y;

         smalldata = new char* [totalheight + 1];

         smalldata[0] = new char[30];
         for (i = 1; i < headersize; i++)
            smalldata[i] = new char [strlen(data[i]) + 1];

         for (i = headersize; i < totalheight + 1; i++)
            smalldata[i] = new char[(new_width * chars) + 1];

         sprintf(smalldata[0], "%u %u %d %d", new_width, new_height, colors, chars);

         for (i = 1; i < headersize; i++) strcpy(smalldata[i], data[i]);

         y = headersize;
         for (y1 = headersize; y1 < (Int_t)new_height + headersize; y1++) {
            x = 0;
            for (x1 = 0; x1 < (Int_t)new_width; x1++) {
               for (pixels = 0; pixels < chars; pixels++)
                  smalldata[y1][x1+pixels] = data[(Int_t)y][(Int_t)x + pixels];
               x += 1.0 / xscale;
            }
            smalldata[y1][x1] = '\0';
            y += 1.0 / yscale;
         }

         retc = gVirtualX->CreatePictureFromData(fClient->GetDefaultRoot()->GetId(), smalldata,
                                                 pic->fPic, pic->fMask,
                                                 pic->fAttributes);

         for (i = 0; i < totalheight + 1; i++)
            delete [] smalldata[i];
         delete [] smalldata;

      } else {
         retc = gVirtualX->CreatePictureFromData(fClient->GetDefaultRoot()->GetId(), data,
                                                 pic->fPic, pic->fMask,
                                                 pic->fAttributes);
      }

      gVirtualX->DeletePictureData(data);
   }

   if (!retc) {
      delete pic;
      return 0;
   }

   fPicList->Add(pic);
   return pic;
}

//______________________________________________________________________________
void TGPicturePool::FreePicture(const TGPicture *fpic)
{
   // Remove picture from cache if nobody is using it anymore.

   if (!fPicList) return;

   TGPicture *pic = (TGPicture *)fPicList->FindObject(fpic);
   if (pic) {
      if (pic->RemoveReference() == 0) {
         fPicList->Remove(pic);
         delete pic;
      }
   }
}

//______________________________________________________________________________
TGPicturePool::~TGPicturePool()
{
   // Delete picture cache.

   if (fPicList) {
      fPicList->Delete();
      delete fPicList;
   }
}

//______________________________________________________________________________
void TGPicturePool::Print(Option_t *) const
{
   // List all pictures in the pool.

   if (fPicList)
      fPicList->Print();
   else
      Info("Print", "no pictures in picture pool");
}

//______________________________________________________________________________
void TGPicture::Draw(Handle_t id, GContext_t gc, Int_t x, Int_t y) const
{
   // Draw a picture.

   GCValues_t gcv;

   gcv.fMask = kGCClipMask | kGCClipXOrigin | kGCClipYOrigin;
   gcv.fClipMask = fMask;
   gcv.fClipXOrigin = x;
   gcv.fClipYOrigin = y;
   gVirtualX->ChangeGC(gc, &gcv);
   gVirtualX->CopyArea(fPic, id, gc, 0, 0, fAttributes.fWidth, fAttributes.fHeight,
                  x, y);
   gcv.fMask = kGCClipMask;
   gcv.fClipMask = kNone;
   gVirtualX->ChangeGC(gc, &gcv);
}

//______________________________________________________________________________
TGPicture::~TGPicture()
{
   // Delete picture object.

   if (fPic != kNone)
      gVirtualX->DeletePixmap(fPic);
   if (fMask != kNone)
      gVirtualX->DeletePixmap(fMask);
   if (fAttributes.fPixels)
      delete [] fAttributes.fPixels;
}

//______________________________________________________________________________
const char *TGPicture::HashName(const char *name, Int_t width, Int_t height)
{
   // Static function returning a unique name used to look up a picture.
   // The unique name has the form "name__widthxheight".

   static char hashName[256];

   sprintf(hashName, "%s__%dx%d", name, width, height);

   return hashName;
}

//______________________________________________________________________________
void TGPicture::Print(Option_t *) const
{
   // Print picture info.

   Printf("TGPicture: %s,%sref cnt = %u", GetName(),
          fScaled ? " scaled, " : " ", References());
}

//______________________________________________________________________________
TGSelectedPicture::TGSelectedPicture(const TGClient *client, const TGPicture *p) :
   TGPicture("")
{
   // Create a "selected" looking picture based on the original TGPicture.

   GCValues_t gcv;
   UInt_t     w, h;

   fClient = client;
   Window_t root  = fClient->GetDefaultRoot()->GetId();

   w = p->GetWidth();
   h = p->GetHeight();

   fPic  = gVirtualX->CreatePixmap(root, w, h);
   fMask = p->GetMask();

   fAttributes.fWidth  = w;
   fAttributes.fHeight = h;

   gVirtualX->CopyArea(p->GetPicture(), fPic, GetSelectedGC()(), 0, 0, w, h, 0, 0);

   gcv.fMask = kGCClipMask | kGCClipXOrigin | kGCClipYOrigin;
   gcv.fClipMask = p->GetMask();
   gcv.fClipXOrigin = 0;
   gcv.fClipYOrigin = 0;
   GetSelectedGC().SetAttributes(&gcv);

   gVirtualX->FillRectangle(fPic, GetSelectedGC()(), 0, 0, w, h);

   GetSelectedGC().SetClipMask(kNone);
}

//______________________________________________________________________________
TGSelectedPicture::~TGSelectedPicture()
{
   // Delete selected picture.

   // fMask was borrowed so should not be deleted by ~TGPicture.
   fMask = kNone;
}

//______________________________________________________________________________
TGGC &TGSelectedPicture::GetSelectedGC()
{
   if (!fgSelectedGC) {
      fgSelectedGC = new TGGC(*gClient->GetResourcePool()->GetFrameGC());
      fgSelectedGC->SetForeground(gClient->GetResourcePool()->GetSelectedBgndColor());
      fgSelectedGC->SetBackground(gClient->GetResourcePool()->GetBlackColor());
      fgSelectedGC->SetFillStyle(kFillStippled);
      fgSelectedGC->SetStipple(gClient->GetResourcePool()->GetCheckeredBitmap());
   }
   return *fgSelectedGC;
}
<commit_msg>ExpandPathName() returns a char* and not a const char*.<commit_after>// @(#)root/gui:$Name:  $:$Id: TGPicture.cxx,v 1.11 2004/08/21 09:32:52 rdm Exp $
// Author: Fons Rademakers   01/01/98

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/
/**************************************************************************

    This source is based on Xclass95, a Win95-looking GUI toolkit.
    Copyright (C) 1996, 1997 David Barth, Ricky Ralston, Hector Peraza.

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

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

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TGPicture & TGPicturePool                                            //
//                                                                      //
// The TGPicture class implements pictures and icons used in the        //
// different GUI elements and widgets. The TGPicturePool class          //
// implements a TGPicture cache. TGPictures are created, managed and    //
// destroyed by the TGPicturePool.                                      //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "TGPicture.h"
#include "TGResourcePool.h"
#include "THashTable.h"
#include "TSystem.h"
#include "TGWindow.h"
#include "TVirtualX.h"

TGGC *TGSelectedPicture::fgSelectedGC = 0;

ClassImp(TGPicture)
ClassImp(TGSelectedPicture)
ClassImp(TGPicturePool)

//______________________________________________________________________________
const TGPicture *TGPicturePool::GetPicture(const char *name)
{
   // Get a picture from the picture pool. Picture must be freed using
   // TGPicturePool::FreePicture(). If picture is not found 0 is returned.

   if (!fPicList)
      fPicList = new THashTable(50);

   TString pname = name;

   if (pname.EndsWith(".xpm")) {
      char *pxname = gSystem->ExpandPathName(gSystem->UnixPathName(pname));
      pname = pxname;
      delete [] pxname;
   }
   TGPicture *pic;

   pic = (TGPicture *)fPicList->FindObject(pname);
   if (pic && !pic->IsScaled()) {
      if (pic->fPic == kNone)
         return 0;
      pic->AddReference();
      return pic;
   }

   pic = new TGPicture(pname);
   pic->fAttributes.fColormap  = fClient->GetDefaultColormap();
   pic->fAttributes.fCloseness = 40000; // Allow for "similar" colors
   pic->fAttributes.fMask      = kPASize | kPAColormap | kPACloseness;

   char *picnam = gSystem->Which(fPath, pname, kReadPermission);
   if (!picnam) {
      fPicList->Add(pic);
      return 0;
   }

   if (gVirtualX->CreatePictureFromFile(fClient->GetDefaultRoot()->GetId(), picnam,
                                        pic->fPic, pic->fMask,
                                        pic->fAttributes)) {
      fPicList->Add(pic);
   } else {
      delete pic;
      pic = new TGPicture(name);
      fPicList->Add(pic);
      pic = 0;
   }

   delete [] picnam;

   return pic;
}

//______________________________________________________________________________
const TGPicture *TGPicturePool::GetPicture(const char *name,
                                           UInt_t new_width, UInt_t new_height)
{
   // Get picture with specified size from pool (picture will be scaled if
   // necessary). Picture must be freed using TGPicturePool::FreePicture(). If
   // picture is not found 0 is returned.

   if (!fPicList)
      fPicList = new THashTable(50);

   TString pname = name;

   if (pname.EndsWith(".xpm")) {
      char *pxname = gSystem->ExpandPathName(gSystem->UnixPathName(pname));
      pname = pxname;
      delete [] pxname;
   }

   TGPicture *pic;

   const char *hname = TGPicture::HashName(pname, new_width, new_height);
   pic = (TGPicture *)fPicList->FindObject(hname);
   if (pic && pic->GetWidth() == new_width && pic->GetHeight() == new_height) {
      if (pic->fPic == kNone)
         return 0;
      pic->AddReference();
      return pic;
   }

   pic = new TGPicture(hname, kTRUE);
   pic->fAttributes.fColormap  = fClient->GetDefaultColormap();
   pic->fAttributes.fCloseness = 40000; // Allow for "similar" colors
   pic->fAttributes.fMask      = kPASize | kPAColormap | kPACloseness;

   char *picnam = gSystem->Which(fPath, pname, kReadPermission);
   if (!picnam) {
      pic->fAttributes.fWidth  = new_width;
      pic->fAttributes.fHeight = new_height;
      fPicList->Add(pic);
      return 0;
   }

   Bool_t retc = kFALSE;
   if (!gVirtualX->InheritsFrom("TGX11")) {
      // case of win32gdk and qt drivers
      retc = gVirtualX->CreatePictureFromFile(fClient->GetDefaultRoot()->GetId(),
                                              picnam, pic->fPic, pic->fMask,
                                              pic->fAttributes);
      delete [] picnam;
   } else {
      // case supported by X11 driver
      char **data;
      if (!gVirtualX->ReadPictureDataFromFile(picnam, &data)) {
         delete pic;
         delete [] picnam;
         return 0;
      }
      delete [] picnam;

      Int_t    colors, chars, headersize, totalheight;
      UInt_t   width, height;
      Double_t xscale, yscale;

      sscanf(data[0], "%u %u %d %d", &width, &height, &colors, &chars);
      headersize = colors + 1;
      yscale = (Double_t) new_height / (Double_t) height;
      xscale = (Double_t) new_width / (Double_t) width;
      totalheight = (colors + new_height + 1);

      if ((width != new_width) || (height != new_height)) {
         char **smalldata;
         Int_t    i, x1, y1, pixels;
         Double_t x, y;

         smalldata = new char* [totalheight + 1];

         smalldata[0] = new char[30];
         for (i = 1; i < headersize; i++)
            smalldata[i] = new char [strlen(data[i]) + 1];

         for (i = headersize; i < totalheight + 1; i++)
            smalldata[i] = new char[(new_width * chars) + 1];

         sprintf(smalldata[0], "%u %u %d %d", new_width, new_height, colors, chars);

         for (i = 1; i < headersize; i++) strcpy(smalldata[i], data[i]);

         y = headersize;
         for (y1 = headersize; y1 < (Int_t)new_height + headersize; y1++) {
            x = 0;
            for (x1 = 0; x1 < (Int_t)new_width; x1++) {
               for (pixels = 0; pixels < chars; pixels++)
                  smalldata[y1][x1+pixels] = data[(Int_t)y][(Int_t)x + pixels];
               x += 1.0 / xscale;
            }
            smalldata[y1][x1] = '\0';
            y += 1.0 / yscale;
         }

         retc = gVirtualX->CreatePictureFromData(fClient->GetDefaultRoot()->GetId(), smalldata,
                                                 pic->fPic, pic->fMask,
                                                 pic->fAttributes);

         for (i = 0; i < totalheight + 1; i++)
            delete [] smalldata[i];
         delete [] smalldata;

      } else {
         retc = gVirtualX->CreatePictureFromData(fClient->GetDefaultRoot()->GetId(), data,
                                                 pic->fPic, pic->fMask,
                                                 pic->fAttributes);
      }

      gVirtualX->DeletePictureData(data);
   }

   if (!retc) {
      delete pic;
      return 0;
   }

   fPicList->Add(pic);
   return pic;
}

//______________________________________________________________________________
void TGPicturePool::FreePicture(const TGPicture *fpic)
{
   // Remove picture from cache if nobody is using it anymore.

   if (!fPicList) return;

   TGPicture *pic = (TGPicture *)fPicList->FindObject(fpic);
   if (pic) {
      if (pic->RemoveReference() == 0) {
         fPicList->Remove(pic);
         delete pic;
      }
   }
}

//______________________________________________________________________________
TGPicturePool::~TGPicturePool()
{
   // Delete picture cache.

   if (fPicList) {
      fPicList->Delete();
      delete fPicList;
   }
}

//______________________________________________________________________________
void TGPicturePool::Print(Option_t *) const
{
   // List all pictures in the pool.

   if (fPicList)
      fPicList->Print();
   else
      Info("Print", "no pictures in picture pool");
}

//______________________________________________________________________________
void TGPicture::Draw(Handle_t id, GContext_t gc, Int_t x, Int_t y) const
{
   // Draw a picture.

   GCValues_t gcv;

   gcv.fMask = kGCClipMask | kGCClipXOrigin | kGCClipYOrigin;
   gcv.fClipMask = fMask;
   gcv.fClipXOrigin = x;
   gcv.fClipYOrigin = y;
   gVirtualX->ChangeGC(gc, &gcv);
   gVirtualX->CopyArea(fPic, id, gc, 0, 0, fAttributes.fWidth, fAttributes.fHeight,
                  x, y);
   gcv.fMask = kGCClipMask;
   gcv.fClipMask = kNone;
   gVirtualX->ChangeGC(gc, &gcv);
}

//______________________________________________________________________________
TGPicture::~TGPicture()
{
   // Delete picture object.

   if (fPic != kNone)
      gVirtualX->DeletePixmap(fPic);
   if (fMask != kNone)
      gVirtualX->DeletePixmap(fMask);
   if (fAttributes.fPixels)
      delete [] fAttributes.fPixels;
}

//______________________________________________________________________________
const char *TGPicture::HashName(const char *name, Int_t width, Int_t height)
{
   // Static function returning a unique name used to look up a picture.
   // The unique name has the form "name__widthxheight".

   static char hashName[256];

   sprintf(hashName, "%s__%dx%d", name, width, height);

   return hashName;
}

//______________________________________________________________________________
void TGPicture::Print(Option_t *) const
{
   // Print picture info.

   Printf("TGPicture: %s,%sref cnt = %u", GetName(),
          fScaled ? " scaled, " : " ", References());
}

//______________________________________________________________________________
TGSelectedPicture::TGSelectedPicture(const TGClient *client, const TGPicture *p) :
   TGPicture("")
{
   // Create a "selected" looking picture based on the original TGPicture.

   GCValues_t gcv;
   UInt_t     w, h;

   fClient = client;
   Window_t root  = fClient->GetDefaultRoot()->GetId();

   w = p->GetWidth();
   h = p->GetHeight();

   fPic  = gVirtualX->CreatePixmap(root, w, h);
   fMask = p->GetMask();

   fAttributes.fWidth  = w;
   fAttributes.fHeight = h;

   gVirtualX->CopyArea(p->GetPicture(), fPic, GetSelectedGC()(), 0, 0, w, h, 0, 0);

   gcv.fMask = kGCClipMask | kGCClipXOrigin | kGCClipYOrigin;
   gcv.fClipMask = p->GetMask();
   gcv.fClipXOrigin = 0;
   gcv.fClipYOrigin = 0;
   GetSelectedGC().SetAttributes(&gcv);

   gVirtualX->FillRectangle(fPic, GetSelectedGC()(), 0, 0, w, h);

   GetSelectedGC().SetClipMask(kNone);
}

//______________________________________________________________________________
TGSelectedPicture::~TGSelectedPicture()
{
   // Delete selected picture.

   // fMask was borrowed so should not be deleted by ~TGPicture.
   fMask = kNone;
}

//______________________________________________________________________________
TGGC &TGSelectedPicture::GetSelectedGC()
{
   if (!fgSelectedGC) {
      fgSelectedGC = new TGGC(*gClient->GetResourcePool()->GetFrameGC());
      fgSelectedGC->SetForeground(gClient->GetResourcePool()->GetSelectedBgndColor());
      fgSelectedGC->SetBackground(gClient->GetResourcePool()->GetBlackColor());
      fgSelectedGC->SetFillStyle(kFillStippled);
      fgSelectedGC->SetStipple(gClient->GetResourcePool()->GetCheckeredBitmap());
   }
   return *fgSelectedGC;
}
<|endoftext|>


<commit_before>/**
 * @file constrained_ik.cpp
 * @brief Basic low-level kinematics functions.
 *
 * Typically, just wrappers around the equivalent KDL calls.
 *
 * @author dsolomon
 * @date Sep 15, 2013
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @license Software License Agreement (Apache License)\n
 * \n
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at\n
 * \n
 * http://www.apache.org/licenses/LICENSE-2.0\n
 * \n
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "constrained_ik/constrained_ik.h"
#include "constrained_ik/constraint_group.h"
#include <ros/ros.h>

namespace constrained_ik
{

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Affine3d;

Constrained_IK::Constrained_IK()
{
  initialized_ = false;
  max_iter_ = 500;                  //default max_iter
  joint_convergence_tol_ = 0.0001;   //default convergence tolerance
  debug_ = false;
  kpp_ = 0.9;// default primary proportional gain
  kpa_ = 0.5;// default auxillary proportional gain
}

Eigen::VectorXd Constrained_IK::calcConstraintError(constraint_types::ConstraintType constraint_type)
{
  switch(constraint_type)
  {
    case constraint_types::primary:
      return primary_constraints_.calcError();
    case constraint_types::auxiliary:
      return auxiliary_constraints_.calcError();
  }
}

Eigen::MatrixXd Constrained_IK::calcConstraintJacobian(constraint_types::ConstraintType constraint_type)
{
  switch(constraint_type)
  {
    case constraint_types::primary:
      return primary_constraints_.calcJacobian();
    case constraint_types::auxiliary:
      return auxiliary_constraints_.calcJacobian();
  }
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjection(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv = calcDampedPseudoinverse(J);
  MatrixXd JplusJ = J_pinv * J;
  int mn = JplusJ.rows();
  MatrixXd P = MatrixXd::Identity(mn,mn)-JplusJ;
  return (P);
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjectionTheRightWay(const Eigen::MatrixXd &A) const
{
  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullV);
  MatrixXd V(svd.matrixV());

  // determine rank using same algorithym as latest Eigen
  // TODO replace next 10 lines of code with rnk = svd.rank(); once eigen3 is updated
  int rnk = 0;
  if(svd.singularValues().size()==0) {
    rnk = 0;
  }
  else{
    double premultiplied_threshold = svd.singularValues().coeff(0) * svd.threshold();
    rnk = svd.nonzeroSingularValues()-1;
    while(rnk>=0 && svd.singularValues().coeff(rnk) < premultiplied_threshold) --rnk;
    rnk++;
  }

  // zero singular vectors in the range of A
  for(int i=0; i<rnk; ++i)
  {
    for(int j=0; j<A.cols(); j++) V(j,i) = 0;
  }
  MatrixXd P = V *  V.transpose();
  return(P);
}

Eigen::MatrixXd Constrained_IK::calcDampedPseudoinverse(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv;

  if (basic_kin::BasicKin::dampedPInv(J,J_pinv)){
    return J_pinv;
  }
  else
  {
    ROS_ERROR_STREAM("Not able to calculate damped pseudoinverse!");
    throw std::runtime_error("Not able to calculate damped pseudoinverse!  IK solution may be invalid.");
  }
}

void Constrained_IK::calcInvKin(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed, Eigen::VectorXd &joint_angles)
{
  if (!checkInitialized(constraint_types::primary))
    throw std::runtime_error("Must call init() before using Constrained_IK");
  //TODO should goal be checked here instead of in reset()?

  // initialize state
  joint_angles = joint_seed;  // initialize result to seed value
  reset(goal, joint_seed);    // reset state vars for this IK solve
  update(joint_angles);       // update current state

  // iterate until solution converges (or aborted)
  while (!checkStatus())
  {

    // calculate a Jacobian (relating joint-space updates/deltas to cartesian-space errors/deltas)
    // and the associated cartesian-space error/delta vector
    // Primary Constraints
    MatrixXd J_p  = calcConstraintJacobian(constraint_types::primary);
    MatrixXd Ji_p = calcDampedPseudoinverse(J_p);
    int rows = J_p.rows();
    int cols = J_p.cols();
    MatrixXd N_p = calcNullspaceProjectionTheRightWay(J_p);
    VectorXd err_p = calcConstraintError(constraint_types::primary);
    // solve for the resulting joint-space update
    VectorXd dJoint_p;
    VectorXd dJoint_a;

    dJoint_p = kpp_*(Ji_p*err_p);
    ROS_DEBUG("theta_p = %f ep = %f",dJoint_p.norm(), err_p.norm());

    // Auxiliary Constraints
    dJoint_a.setZero(dJoint_p.size());
    if (checkInitialized(constraint_types::auxiliary)) {
      MatrixXd J_a  = calcConstraintJacobian(constraint_types::auxiliary);
      VectorXd err_a = calcConstraintError(constraint_types::auxiliary);
      MatrixXd Jnull_a = calcDampedPseudoinverse(J_a*N_p);
      dJoint_a = kpa_*Jnull_a*(err_a-J_a*dJoint_p);
      ROS_DEBUG("theta_p = %f ep = %f ea = %f",dJoint_p.norm(), err_p.norm(), err_a.norm());
    }

    // update joint solution by the calculated update (or a partial fraction)
    joint_angles += (dJoint_p + dJoint_a);
    clipToJointLimits(joint_angles);

    // re-update internal state variables
    update(joint_angles);
  }

  ROS_INFO_STREAM("IK solution: " << joint_angles.transpose());
}

bool Constrained_IK::checkStatus() const
{
  // check constraints for completion
  if (primary_constraints_.checkStatus() && auxiliary_constraints_.checkStatus())
    return true;

  // check maximum iterations
  if (state_.iter > max_iter_)
    throw std::runtime_error("Maximum iterations reached.  IK solution may be invalid.");

  // check for joint convergence
  //   - this is an error: joints stabilize, but goal pose not reached
  if (state_.joints_delta.cwiseAbs().maxCoeff() < joint_convergence_tol_)
  {
//	  ROS_ERROR_STREAM("Reached " << state_.iter << " / " << max_iter_ << " iterations before convergence.");
//	  throw std::runtime_error("Iteration converged before goal reached.  IK solution may be invalid");

	  ROS_WARN_STREAM("Reached " << state_.iter << " / " << max_iter_ << " iterations before convergence.");
      return true;
  }

  return false;
}

void Constrained_IK::clearConstraintList()
{
  primary_constraints_.clear();
  auxiliary_constraints_.clear();
}

void Constrained_IK::clipToJointLimits(Eigen::VectorXd &joints)
{
  const MatrixXd limits = kin_.getLimits();
  const VectorXd orig_joints(joints);

  if (joints.size() != limits.rows())
    throw std::invalid_argument("clipToJointLimits: Unexpected number of joints");

  for (size_t i=0; i<limits.rows(); ++i)
  {
    joints[i] = std::max(limits(i,0), std::min(limits(i,1), joints[i]));
  }
  if (debug_ && !joints.isApprox(orig_joints))
      ROS_WARN("Joints have been clipped");
}

void Constrained_IK::init(const urdf::Model &robot, const std::string &base_name, const std::string &tip_name)
{
  basic_kin::BasicKin kin;
  if (! kin.init(robot, base_name, tip_name))
    throw std::runtime_error("Failed to initialize BasicKin");

  init(kin);
}

void Constrained_IK::init(const basic_kin::BasicKin &kin)
{
  if (!kin.checkInitialized())
    throw std::invalid_argument("Input argument 'BasicKin' must be initialized");

  kin_ = kin;
  initialized_ = true;
  primary_constraints_.init(this);
  auxiliary_constraints_.init(this);
}

double Constrained_IK::rangedAngle(double angle)
{
    angle = copysign(fmod(fabs(angle),2.0*M_PI), angle);
    if (angle < -M_PI) return angle+2.*M_PI;
    if (angle > M_PI)  return angle-2.*M_PI;
    return angle;
}

void Constrained_IK::reset(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed)
{
  if (!kin_.checkJoints(joint_seed))
    throw std::invalid_argument("Seed doesn't match kinematic model");

  if (!goal.matrix().block(0,0,3,3).isUnitary(1e-6))
        throw std::invalid_argument("Goal pose not proper affine");

  state_.reset(goal, joint_seed);  // reset state
  primary_constraints_.reset();            // reset primary constraints
  auxiliary_constraints_.reset();            // reset auxiliary constraints
}

void Constrained_IK::update(const Eigen::VectorXd &joints)
{
  // update maximum iterations
  state_.iter++;

  // update joint convergence
  state_.joints_delta = joints - state_.joints;
  state_.joints = joints;
  kin_.calcFwdKin(joints, state_.pose_estimate);

  if (debug_)
      iteration_path_.push_back(joints);

  primary_constraints_.update(state_);
  auxiliary_constraints_.update(state_);
}

} // namespace constrained_ik

<commit_msg>put all leading { on their own line to waste screen space<commit_after>/**
 * @file constrained_ik.cpp
 * @brief Basic low-level kinematics functions.
 *
 * Typically, just wrappers around the equivalent KDL calls.
 *
 * @author dsolomon
 * @date Sep 15, 2013
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @license Software License Agreement (Apache License)\n
 * \n
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at\n
 * \n
 * http://www.apache.org/licenses/LICENSE-2.0\n
 * \n
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "constrained_ik/constrained_ik.h"
#include "constrained_ik/constraint_group.h"
#include <ros/ros.h>

namespace constrained_ik
{

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Affine3d;

Constrained_IK::Constrained_IK()
{
  initialized_ = false;
  max_iter_ = 500;                  //default max_iter
  joint_convergence_tol_ = 0.0001;   //default convergence tolerance
  debug_ = false;
  kpp_ = 0.9;// default primary proportional gain
  kpa_ = 0.5;// default auxillary proportional gain
}

Eigen::VectorXd Constrained_IK::calcConstraintError(constraint_types::ConstraintType constraint_type)
{
  switch(constraint_type)
  {
    case constraint_types::primary:
      return primary_constraints_.calcError();
    case constraint_types::auxiliary:
      return auxiliary_constraints_.calcError();
  }
}

Eigen::MatrixXd Constrained_IK::calcConstraintJacobian(constraint_types::ConstraintType constraint_type)
{
  switch(constraint_type)
  {
    case constraint_types::primary:
      return primary_constraints_.calcJacobian();
    case constraint_types::auxiliary:
      return auxiliary_constraints_.calcJacobian();
  }
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjection(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv = calcDampedPseudoinverse(J);
  MatrixXd JplusJ = J_pinv * J;
  int mn = JplusJ.rows();
  MatrixXd P = MatrixXd::Identity(mn,mn)-JplusJ;
  return (P);
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjectionTheRightWay(const Eigen::MatrixXd &A) const
{
  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullV);
  MatrixXd V(svd.matrixV());

  // determine rank using same algorithym as latest Eigen
  // TODO replace next 10 lines of code with rnk = svd.rank(); once eigen3 is updated
  int rnk = 0;
  if(svd.singularValues().size()==0)
    {
      rnk = 0;
    }
  else
    {
      double premultiplied_threshold = svd.singularValues().coeff(0) * svd.threshold();
      rnk = svd.nonzeroSingularValues()-1;
      while(rnk>=0 && svd.singularValues().coeff(rnk) < premultiplied_threshold) --rnk;
      rnk++;
    }

  // zero singular vectors in the range of A
  for(int i=0; i<rnk; ++i)
  {
    for(int j=0; j<A.cols(); j++) V(j,i) = 0;
  }
  MatrixXd P = V *  V.transpose();
  return(P);
}

Eigen::MatrixXd Constrained_IK::calcDampedPseudoinverse(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv;

  if (basic_kin::BasicKin::dampedPInv(J,J_pinv))
    {
      return J_pinv;
    }
  else
    {
      ROS_ERROR_STREAM("Not able to calculate damped pseudoinverse!");
      throw std::runtime_error("Not able to calculate damped pseudoinverse!  IK solution may be invalid.");
    }
}

void Constrained_IK::calcInvKin(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed, Eigen::VectorXd &joint_angles)
{
  if (!checkInitialized(constraint_types::primary))
    throw std::runtime_error("Must call init() before using Constrained_IK");
  //TODO should goal be checked here instead of in reset()?

  // initialize state
  joint_angles = joint_seed;  // initialize result to seed value
  reset(goal, joint_seed);    // reset state vars for this IK solve
  update(joint_angles);       // update current state

  // iterate until solution converges (or aborted)
  while (!checkStatus())
  {

    // calculate a Jacobian (relating joint-space updates/deltas to cartesian-space errors/deltas)
    // and the associated cartesian-space error/delta vector
    // Primary Constraints
    MatrixXd J_p  = calcConstraintJacobian(constraint_types::primary);
    MatrixXd Ji_p = calcDampedPseudoinverse(J_p);
    int rows = J_p.rows();
    int cols = J_p.cols();
    MatrixXd N_p = calcNullspaceProjectionTheRightWay(J_p);
    VectorXd err_p = calcConstraintError(constraint_types::primary);
    // solve for the resulting joint-space update
    VectorXd dJoint_p;
    VectorXd dJoint_a;

    dJoint_p = kpp_*(Ji_p*err_p);
    ROS_DEBUG("theta_p = %f ep = %f",dJoint_p.norm(), err_p.norm());

    // Auxiliary Constraints
    dJoint_a.setZero(dJoint_p.size());
    if (checkInitialized(constraint_types::auxiliary))
      {
	MatrixXd J_a  = calcConstraintJacobian(constraint_types::auxiliary);
	VectorXd err_a = calcConstraintError(constraint_types::auxiliary);
	MatrixXd Jnull_a = calcDampedPseudoinverse(J_a*N_p);
	dJoint_a = kpa_*Jnull_a*(err_a-J_a*dJoint_p);
	ROS_DEBUG("theta_p = %f ep = %f ea = %f",dJoint_p.norm(), err_p.norm(), err_a.norm());
      }

    // update joint solution by the calculated update (or a partial fraction)
    joint_angles += (dJoint_p + dJoint_a);
    clipToJointLimits(joint_angles);

    // re-update internal state variables
    update(joint_angles);
  }

  ROS_INFO_STREAM("IK solution: " << joint_angles.transpose());
}

bool Constrained_IK::checkStatus() const
{
  // check constraints for completion
  if (primary_constraints_.checkStatus() && auxiliary_constraints_.checkStatus())
    return true;

  // check maximum iterations
  if (state_.iter > max_iter_)
    throw std::runtime_error("Maximum iterations reached.  IK solution may be invalid.");

  // check for joint convergence
  //   - this is an error: joints stabilize, but goal pose not reached
  if (state_.joints_delta.cwiseAbs().maxCoeff() < joint_convergence_tol_)
  {
//	  ROS_ERROR_STREAM("Reached " << state_.iter << " / " << max_iter_ << " iterations before convergence.");
//	  throw std::runtime_error("Iteration converged before goal reached.  IK solution may be invalid");

	  ROS_WARN_STREAM("Reached " << state_.iter << " / " << max_iter_ << " iterations before convergence.");
      return true;
  }

  return false;
}

void Constrained_IK::clearConstraintList()
{
  primary_constraints_.clear();
  auxiliary_constraints_.clear();
}

void Constrained_IK::clipToJointLimits(Eigen::VectorXd &joints)
{
  const MatrixXd limits = kin_.getLimits();
  const VectorXd orig_joints(joints);

  if (joints.size() != limits.rows())
    throw std::invalid_argument("clipToJointLimits: Unexpected number of joints");

  for (size_t i=0; i<limits.rows(); ++i)
  {
    joints[i] = std::max(limits(i,0), std::min(limits(i,1), joints[i]));
  }
  if (debug_ && !joints.isApprox(orig_joints))
      ROS_WARN("Joints have been clipped");
}

void Constrained_IK::init(const urdf::Model &robot, const std::string &base_name, const std::string &tip_name)
{
  basic_kin::BasicKin kin;
  if (! kin.init(robot, base_name, tip_name))
    throw std::runtime_error("Failed to initialize BasicKin");

  init(kin);
}

void Constrained_IK::init(const basic_kin::BasicKin &kin)
{
  if (!kin.checkInitialized())
    throw std::invalid_argument("Input argument 'BasicKin' must be initialized");

  kin_ = kin;
  initialized_ = true;
  primary_constraints_.init(this);
  auxiliary_constraints_.init(this);
}

double Constrained_IK::rangedAngle(double angle)
{
    angle = copysign(fmod(fabs(angle),2.0*M_PI), angle);
    if (angle < -M_PI) return angle+2.*M_PI;
    if (angle > M_PI)  return angle-2.*M_PI;
    return angle;
}

void Constrained_IK::reset(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed)
{
  if (!kin_.checkJoints(joint_seed))
    throw std::invalid_argument("Seed doesn't match kinematic model");

  if (!goal.matrix().block(0,0,3,3).isUnitary(1e-6))
        throw std::invalid_argument("Goal pose not proper affine");

  state_.reset(goal, joint_seed);  // reset state
  primary_constraints_.reset();            // reset primary constraints
  auxiliary_constraints_.reset();            // reset auxiliary constraints
}

void Constrained_IK::update(const Eigen::VectorXd &joints)
{
  // update maximum iterations
  state_.iter++;

  // update joint convergence
  state_.joints_delta = joints - state_.joints;
  state_.joints = joints;
  kin_.calcFwdKin(joints, state_.pose_estimate);

  if (debug_)
      iteration_path_.push_back(joints);

  primary_constraints_.update(state_);
  auxiliary_constraints_.update(state_);
}

} // namespace constrained_ik

<|endoftext|>


<commit_before>/** Copyright (C) 2016, 2017 European Spallation Source ERIC */

#include <readout/ReadoutDummy.h>
#include <readout/ReadoutTestData.h>
#include <test/TestBase.h>

#define DETECTOR_TYPE 1

class ReadoutDummyTest : public TestBase {
protected:
  char buffer[9000];
  virtual void SetUp() {
    memset(buffer, 0, sizeof(buffer));
  }
  virtual void TearDown() {}
};

TEST_F(ReadoutDummyTest, Parser) {
  ReadoutDummy readout(DETECTOR_TYPE);

  int size = ok_one_hit.size();

  auto res = readout.parse((char *)&ok_one_hit[0], size);
  ASSERT_EQ(res, 0);
}

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<commit_msg>added a couple of tests<commit_after>/** Copyright (C) 2016, 2017 European Spallation Source ERIC */

#include <readout/ReadoutDummy.h>
#include <readout/ReadoutTestData.h>
#include <test/TestBase.h>

#define DETECTOR_TYPE 1

class ReadoutDummyTest : public TestBase {
protected:
  char buffer[9000];
  virtual void SetUp() {
    memset(buffer, 0, sizeof(buffer));
  }
  virtual void TearDown() {}
};

TEST_F(ReadoutDummyTest, ValidateFail) {
  ReadoutDummy readout(DETECTOR_TYPE);
  auto res = readout.parse(0, 64);
  ASSERT_EQ(res, -1);
}

TEST_F(ReadoutDummyTest, TypeFail) {
  ReadoutDummy readout(DETECTOR_TYPE + 1);
  int size = ok_one_hit.size();

  auto res = readout.parse((char *)&ok_one_hit[0], size);
  ASSERT_EQ(res, -1);
}

TEST_F(ReadoutDummyTest, Parser) {
  ReadoutDummy readout(DETECTOR_TYPE);

  int size = ok_one_hit.size();

  auto res = readout.parse((char *)&ok_one_hit[0], size);
  ASSERT_EQ(res, 0);
}

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<|endoftext|>


<commit_before>/*
	Copyright (c) 2014, Madd Games.
	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.

	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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <Apoc/Entity/Entity.h>
#include <Apoc/Entity/World.h>
#include <Apoc/Utils/Utils.h>
#include <Apoc/Physics/CollisionCheck.h>
#include <inttypes.h>
#include <Apoc/Video/RenderHandler.h>
#include <Apoc/Utils/Archive.h>
#include <string.h>
#include <iostream>
#include <stddef.h>

using namespace std;

extern RenderHandler *apocRenderHandler;
map<string, map<string, Entity::Object> > Entity::apocEntityCache;

// XXX: Don't worry guys, this constructor is going away soon.
Entity::Entity(Model::ObjDef *defs) : bbDirty(true), shouldRemove(false), isStatic(true), entParent(NULL)
{
	cerr << "[APOC] [WARNING] The deprecated Entity constructor was used to load an embedded OBJ file!" << endl;

	modelMatrix = Matrix::Identity();

	while (defs->vertices != NULL)
	{
		Object obj;
		if (defs->model == NULL)
		{
			obj.model = new Model(defs->vertices, defs->count);
			defs->model = obj.model;
		}
		else
		{
			obj.model = defs->model;
		};

		string texName = defs->texName;
		string specMap = defs->specTex;
		string normalMap = defs->normalMap;
		if (texName != "empty_texture")
		{
			obj.textures[0] = Texture::Get(texName);
		};
		if (specMap != "<NONE>")
		{
			obj.textures[4] = Texture::Get(specMap);
		};
		if (normalMap != "<NONE>")
		{
			obj.textures[5] = Texture::Get(normalMap);
		};
		obj.matrix = Matrix::Identity();
		obj.diffuseColor = defs->diffuseColor;
		obj.specularColor = defs->specularColor;
		obj.shininess = defs->shininess;
		string name(defs->name);
		obj.visible = (name != "ApocColl");
		obj.collideable = (name == "ApocColl");
		objects[defs->name] = obj;
		defs++;
	};

	if (objects.count("ApocColl") == 0)
	{
		map<string, Object>::iterator it;
		for (it=objects.begin(); it!=objects.end(); ++it)
		{
			it->second.collideable = true;
		};
	};
};

Entity::Entity(string entname) : bbDirty(true), shouldRemove(false), isStatic(true), entParent(NULL)
{
	modelMatrix = Matrix::Identity();

	if (apocEntityCache.count(entname) != 0)
	{
		objects = apocEntityCache[entname];
	}
	else
	{
		DataFile file(string("Game/Models/") + entname + ".apm");
		uint8_t *data = new uint8_t[file.getSize()];
		file.read(data, file.getSize());

		APM_Header *header = (APM_Header*) data;
		if (memcmp(header->magic, "APM", 3) != 0)
		{
			ApocFail(entname + " does not have a valid APM header");
		};

		map<uint32_t, Texture*> modelTextures;
		uint32_t count = header->numTex;
		APM_TexHeader *texHeader = (APM_TexHeader*) &data[header->offTex];

		while (count--)
		{
			uint8_t *texdata = (uint8_t*) (texHeader+1);
			modelTextures[texHeader->idx] = new Texture(texHeader->width, texHeader->height, texdata, (texHeader->flags & 1) == 0);
			texHeader = (APM_TexHeader*) &texdata[4 * texHeader->width * texHeader->height];
		};

		count = header->numObjDefs;
		uint8_t *objscan = &data[header->offObjDefs];

		while (count--)
		{
			string name((const char*)objscan);
			objscan += name.size();
			objscan++;
			APM_ObjHeader *objHeader = (APM_ObjHeader*) objscan;

			// unroll the vertices
			Model::Vertex *vertices = new Model::Vertex[objHeader->numVertices];
			Model::Vertex *vunroll = vertices;
			uint8_t *mdlv = (uint8_t*) objHeader + objHeader->szThis;
			uint32_t vcnt = objHeader->numVertices;

			while (vcnt--)
			{
				memcpy(vunroll, mdlv, header->szVertex);
				vunroll++;
				mdlv += header->szVertex;
			};

			uint32_t idxIllumMap = 0;
			uint32_t idxWarpMap = 0;

			if (objHeader->szThis >= 36)
			{
				idxIllumMap = objHeader->idxIllumMap;
			};

			if (objHeader->szThis >= 40)
			{
				idxWarpMap = objHeader->idxWarpMap;
			};

			Object obj;
			obj.model = new Model(vertices, objHeader->numVertices);
			obj.matrix = Matrix::Identity();
			if (objHeader->idxColor != 0)
			{
				obj.textures[0] = modelTextures[objHeader->idxColor];
			}
			else
			{
				cerr << "[APOC] [MODEL] WARNING: " << entname << " contains an object (" << name << ") with no color map" << endl;
			};

			if (objHeader->idxSpecular != 0)
			{
				obj.textures[4] = modelTextures[objHeader->idxSpecular];
			};
			if (objHeader->idxNormals != 0)
			{
				obj.textures[5] = modelTextures[objHeader->idxNormals];
			};
			if (idxIllumMap != 0)
			{
				obj.textures[6] = modelTextures[idxIllumMap];
			};
			if (idxWarpMap != 0)
			{
				obj.textures[7] = modelTextures[idxWarpMap];
			};
			obj.diffuseColor = Vector(
				(float) objHeader->colDiffuse[0] / 255.0,
				(float) objHeader->colDiffuse[1] / 255.0,
				(float) objHeader->colDiffuse[2] / 255.0,
				(float) objHeader->colDiffuse[3] / 255.0
			);
			obj.specularColor = Vector(
				(float) objHeader->colSpecular[0] / 255.0,
				(float) objHeader->colSpecular[1] / 255.0,
				(float) objHeader->colSpecular[2] / 255.0,
				(float) objHeader->colSpecular[3] / 255.0
			);
			obj.shininess = objHeader->shininess;
			obj.visible = (name != "ApocColl");
			obj.collideable = (name == "ApocColl");
			objects[name] = obj;

			objscan = mdlv;
		};

		delete data;

		if (objects.count("ApocColl") == 0)
		{
			map<string, Object>::iterator it;
			for (it=objects.begin(); it!=objects.end(); ++it)
			{
				it->second.collideable = true;
			};
		};

		apocEntityCache[entname] = objects;
	};
};

Entity::~Entity()
{
};

void Entity::transform(string obj, Matrix mat)
{
	if (obj == "")
	{
		modelMatrix = mat * modelMatrix;
	}
	else
	{
		objects[obj].matrix = mat * objects[obj].matrix;
	};
	bbDirty = true;
	isStatic = false;

	vector<IBindable*>::iterator it;
	for (it=boundList.begin(); it!=boundList.end(); ++it)
	{
		(*it)->transform(getModelMatrix());
	};
};

void Entity::preTransform(string obj, Matrix mat)
{
	if (obj == "")
	{
		modelMatrix = modelMatrix * mat;
	}
	else
	{
		objects[obj].matrix = objects[obj].matrix * mat;
	};
	bbDirty = true;
	isStatic = false;

	vector<IBindable*>::iterator it;
	for (it=boundList.begin(); it!=boundList.end(); ++it)
	{
		(*it)->transform(getModelMatrix());
	};
};

Matrix Entity::getModelMatrix()
{
	if (entParent != NULL)
	{
		return entParent->getModelMatrix() * modelMatrix;
	};
	return modelMatrix;
};

bool& Entity::visible(string name)
{
	return objects[name].visible;
};

bool& Entity::collideable(string name)
{
	return objects[name].collideable;
};

void Entity::update()
{
};

void Entity::renderObjects()
{
	GLint uModelMatrix = apocRenderHandler->getUniformLocation("uModelMatrix");
	GLint uObjectMatrix = apocRenderHandler->getUniformLocation("uObjectMatrix");
	GLint uDiffuseColor = apocRenderHandler->getUniformLocation("uDiffuseColor");
	GLint uSpecularColor = apocRenderHandler->getUniformLocation("uSpecularColor");
	GLint uShininess = apocRenderHandler->getUniformLocation("uShininess");

	Matrix matModel = getModelMatrix();
	glUniformMatrix4fv(uModelMatrix, 1, GL_FALSE, &matModel[0][0]);

	map<string, Object>::iterator it;
	for (it=objects.begin(); it!=objects.end(); ++it)
	{
		if (it->second.visible)
		{
			apocRenderHandler->bindDefaultTextures();
			map<unsigned int, Texture*>::iterator jt;
			for (jt=it->second.textures.begin(); jt!=it->second.textures.end(); ++jt)
			{
				glActiveTexture(GL_TEXTURE0 + jt->first);
				jt->second->bind();
			};

			glUniform4fv(uDiffuseColor, 1, &it->second.diffuseColor[0]);
			glUniform4fv(uSpecularColor, 1, &it->second.specularColor[0]);
			glUniform1f(uShininess, it->second.shininess);
			glUniformMatrix4fv(uObjectMatrix, 1, GL_FALSE, &it->second.matrix[0][0]);
			it->second.model->draw();
		};
	};
};

void Entity::translate(Vector vec, const char *objName)
{
	transform(objName, Matrix::Translate(vec.x(), vec.y(), vec.z()));
};

void Entity::rotate(float x, float y, float z, const char *objName)
{
	preTransform(objName, Matrix::Rotate(x, y, z));
};

void Entity::markForRemoval()
{
	shouldRemove = true;
};

void Entity::attachTo(Entity *parent)
{
	entParent = parent;
};

void Entity::bind(IBindable *obj)
{
	obj->transform(getModelMatrix());
	boundList.push_back(obj);
};

void Entity::unmangleVectors(Vector &a, Vector &b)
{
	int i;
	for (i=0; i<3; i++)
	{
		if (a[i] > b[i])
		{
			float tmp = a[i];
			a[i] = b[i];
			b[i] = tmp;
		};
	};
};

Entity::BoundingBox Entity::getBoundingBox()
{
	if (bbDirty)
	{
		modelBoundingBox.min = modelMatrix * objects.begin()->second.matrix * objects.begin()->second.model->data[0].pos;
		modelBoundingBox.max = modelBoundingBox.min;

		map<string, Object>::iterator it;
		for (it=objects.begin(); it!=objects.end(); ++it)
		{
			Matrix mat = modelMatrix * it->second.matrix;
			int i;
			for (i=0; i<it->second.model->vertexCount; i++)
			{
				Vector pos = mat * it->second.model->data[i].pos;
				int j;
				for (j=0; j<3; j++)
				{
					if (pos[j] < modelBoundingBox.min[j]) modelBoundingBox.min[j] = pos[j];
					if (pos[j] > modelBoundingBox.max[j]) modelBoundingBox.max[j] = pos[j];
				};
			};
		};

		bbDirty = false;
	};

	return modelBoundingBox;
};

Entity* Entity::checkCollision()
{
	vector<Entity*>::iterator it;
	for (it=World::entities.begin(); it!=World::entities.end(); ++it)
	{
		if ((*it) != this)
		{
			if (CollisionCheck::Entities(this, *it))
			{
				return *it;
			};
		};
	};

	return NULL;
};

Entity* Entity::move(Vector vec)
{
	translate(vec);
	Entity *coll = checkCollision();
	if (coll != NULL)
	{
		translate(-vec);
		return coll;
	};
	return NULL;
};
<commit_msg>APM Version Check<commit_after>/*
	Copyright (c) 2014, Madd Games.
	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.

	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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <Apoc/Entity/Entity.h>
#include <Apoc/Entity/World.h>
#include <Apoc/Utils/Utils.h>
#include <Apoc/Physics/CollisionCheck.h>
#include <inttypes.h>
#include <Apoc/Video/RenderHandler.h>
#include <Apoc/Utils/Archive.h>
#include <string.h>
#include <iostream>
#include <stddef.h>

using namespace std;

extern RenderHandler *apocRenderHandler;
map<string, map<string, Entity::Object> > Entity::apocEntityCache;

// XXX: Don't worry guys, this constructor is going away soon.
Entity::Entity(Model::ObjDef *defs) : bbDirty(true), shouldRemove(false), isStatic(true), entParent(NULL)
{
	cerr << "[APOC] [WARNING] The deprecated Entity constructor was used to load an embedded OBJ file!" << endl;

	modelMatrix = Matrix::Identity();

	while (defs->vertices != NULL)
	{
		Object obj;
		if (defs->model == NULL)
		{
			obj.model = new Model(defs->vertices, defs->count);
			defs->model = obj.model;
		}
		else
		{
			obj.model = defs->model;
		};

		string texName = defs->texName;
		string specMap = defs->specTex;
		string normalMap = defs->normalMap;
		if (texName != "empty_texture")
		{
			obj.textures[0] = Texture::Get(texName);
		};
		if (specMap != "<NONE>")
		{
			obj.textures[4] = Texture::Get(specMap);
		};
		if (normalMap != "<NONE>")
		{
			obj.textures[5] = Texture::Get(normalMap);
		};
		obj.matrix = Matrix::Identity();
		obj.diffuseColor = defs->diffuseColor;
		obj.specularColor = defs->specularColor;
		obj.shininess = defs->shininess;
		string name(defs->name);
		obj.visible = (name != "ApocColl");
		obj.collideable = (name == "ApocColl");
		objects[defs->name] = obj;
		defs++;
	};

	if (objects.count("ApocColl") == 0)
	{
		map<string, Object>::iterator it;
		for (it=objects.begin(); it!=objects.end(); ++it)
		{
			it->second.collideable = true;
		};
	};
};

Entity::Entity(string entname) : bbDirty(true), shouldRemove(false), isStatic(true), entParent(NULL)
{
	modelMatrix = Matrix::Identity();

	if (apocEntityCache.count(entname) != 0)
	{
		objects = apocEntityCache[entname];
	}
	else
	{
		DataFile file(string("Game/Models/") + entname + ".apm");
		uint8_t *data = new uint8_t[file.getSize()];
		file.read(data, file.getSize());

		APM_Header *header = (APM_Header*) data;
		if (memcmp(header->magic, "APM", 3) != 0)
		{
			ApocFail(entname + " does not have a valid APM header");
		};

		if (header->rev != 0)
		{
			stringstream ss;
			ss << entname << " has an unsupported revision: " << header->rev;
			ApocFail(ss.str());
		};

		map<uint32_t, Texture*> modelTextures;
		uint32_t count = header->numTex;
		APM_TexHeader *texHeader = (APM_TexHeader*) &data[header->offTex];

		while (count--)
		{
			uint8_t *texdata = (uint8_t*) (texHeader+1);
			modelTextures[texHeader->idx] = new Texture(texHeader->width, texHeader->height, texdata, (texHeader->flags & 1) == 0);
			texHeader = (APM_TexHeader*) &texdata[4 * texHeader->width * texHeader->height];
		};

		count = header->numObjDefs;
		uint8_t *objscan = &data[header->offObjDefs];

		while (count--)
		{
			string name((const char*)objscan);
			objscan += name.size();
			objscan++;
			APM_ObjHeader *objHeader = (APM_ObjHeader*) objscan;

			// unroll the vertices
			Model::Vertex *vertices = new Model::Vertex[objHeader->numVertices];
			Model::Vertex *vunroll = vertices;
			uint8_t *mdlv = (uint8_t*) objHeader + objHeader->szThis;
			uint32_t vcnt = objHeader->numVertices;

			while (vcnt--)
			{
				memcpy(vunroll, mdlv, header->szVertex);
				vunroll++;
				mdlv += header->szVertex;
			};

			uint32_t idxIllumMap = 0;
			uint32_t idxWarpMap = 0;

			if (objHeader->szThis >= 36)
			{
				idxIllumMap = objHeader->idxIllumMap;
			};

			if (objHeader->szThis >= 40)
			{
				idxWarpMap = objHeader->idxWarpMap;
			};

			Object obj;
			obj.model = new Model(vertices, objHeader->numVertices);
			obj.matrix = Matrix::Identity();
			if (objHeader->idxColor != 0)
			{
				obj.textures[0] = modelTextures[objHeader->idxColor];
			}
			else
			{
				cerr << "[APOC] [MODEL] WARNING: " << entname << " contains an object (" << name << ") with no color map" << endl;
			};

			if (objHeader->idxSpecular != 0)
			{
				obj.textures[4] = modelTextures[objHeader->idxSpecular];
			};
			if (objHeader->idxNormals != 0)
			{
				obj.textures[5] = modelTextures[objHeader->idxNormals];
			};
			if (idxIllumMap != 0)
			{
				obj.textures[6] = modelTextures[idxIllumMap];
			};
			if (idxWarpMap != 0)
			{
				obj.textures[7] = modelTextures[idxWarpMap];
			};
			obj.diffuseColor = Vector(
				(float) objHeader->colDiffuse[0] / 255.0,
				(float) objHeader->colDiffuse[1] / 255.0,
				(float) objHeader->colDiffuse[2] / 255.0,
				(float) objHeader->colDiffuse[3] / 255.0
			);
			obj.specularColor = Vector(
				(float) objHeader->colSpecular[0] / 255.0,
				(float) objHeader->colSpecular[1] / 255.0,
				(float) objHeader->colSpecular[2] / 255.0,
				(float) objHeader->colSpecular[3] / 255.0
			);
			obj.shininess = objHeader->shininess;
			obj.visible = (name != "ApocColl");
			obj.collideable = (name == "ApocColl");
			objects[name] = obj;

			objscan = mdlv;
		};

		delete data;

		if (objects.count("ApocColl") == 0)
		{
			map<string, Object>::iterator it;
			for (it=objects.begin(); it!=objects.end(); ++it)
			{
				it->second.collideable = true;
			};
		};

		apocEntityCache[entname] = objects;
	};
};

Entity::~Entity()
{
};

void Entity::transform(string obj, Matrix mat)
{
	if (obj == "")
	{
		modelMatrix = mat * modelMatrix;
	}
	else
	{
		objects[obj].matrix = mat * objects[obj].matrix;
	};
	bbDirty = true;
	isStatic = false;

	vector<IBindable*>::iterator it;
	for (it=boundList.begin(); it!=boundList.end(); ++it)
	{
		(*it)->transform(getModelMatrix());
	};
};

void Entity::preTransform(string obj, Matrix mat)
{
	if (obj == "")
	{
		modelMatrix = modelMatrix * mat;
	}
	else
	{
		objects[obj].matrix = objects[obj].matrix * mat;
	};
	bbDirty = true;
	isStatic = false;

	vector<IBindable*>::iterator it;
	for (it=boundList.begin(); it!=boundList.end(); ++it)
	{
		(*it)->transform(getModelMatrix());
	};
};

Matrix Entity::getModelMatrix()
{
	if (entParent != NULL)
	{
		return entParent->getModelMatrix() * modelMatrix;
	};
	return modelMatrix;
};

bool& Entity::visible(string name)
{
	return objects[name].visible;
};

bool& Entity::collideable(string name)
{
	return objects[name].collideable;
};

void Entity::update()
{
};

void Entity::renderObjects()
{
	GLint uModelMatrix = apocRenderHandler->getUniformLocation("uModelMatrix");
	GLint uObjectMatrix = apocRenderHandler->getUniformLocation("uObjectMatrix");
	GLint uDiffuseColor = apocRenderHandler->getUniformLocation("uDiffuseColor");
	GLint uSpecularColor = apocRenderHandler->getUniformLocation("uSpecularColor");
	GLint uShininess = apocRenderHandler->getUniformLocation("uShininess");

	Matrix matModel = getModelMatrix();
	glUniformMatrix4fv(uModelMatrix, 1, GL_FALSE, &matModel[0][0]);

	map<string, Object>::iterator it;
	for (it=objects.begin(); it!=objects.end(); ++it)
	{
		if (it->second.visible)
		{
			apocRenderHandler->bindDefaultTextures();
			map<unsigned int, Texture*>::iterator jt;
			for (jt=it->second.textures.begin(); jt!=it->second.textures.end(); ++jt)
			{
				glActiveTexture(GL_TEXTURE0 + jt->first);
				jt->second->bind();
			};

			glUniform4fv(uDiffuseColor, 1, &it->second.diffuseColor[0]);
			glUniform4fv(uSpecularColor, 1, &it->second.specularColor[0]);
			glUniform1f(uShininess, it->second.shininess);
			glUniformMatrix4fv(uObjectMatrix, 1, GL_FALSE, &it->second.matrix[0][0]);
			it->second.model->draw();
		};
	};
};

void Entity::translate(Vector vec, const char *objName)
{
	transform(objName, Matrix::Translate(vec.x(), vec.y(), vec.z()));
};

void Entity::rotate(float x, float y, float z, const char *objName)
{
	preTransform(objName, Matrix::Rotate(x, y, z));
};

void Entity::markForRemoval()
{
	shouldRemove = true;
};

void Entity::attachTo(Entity *parent)
{
	entParent = parent;
};

void Entity::bind(IBindable *obj)
{
	obj->transform(getModelMatrix());
	boundList.push_back(obj);
};

void Entity::unmangleVectors(Vector &a, Vector &b)
{
	int i;
	for (i=0; i<3; i++)
	{
		if (a[i] > b[i])
		{
			float tmp = a[i];
			a[i] = b[i];
			b[i] = tmp;
		};
	};
};

Entity::BoundingBox Entity::getBoundingBox()
{
	if (bbDirty)
	{
		modelBoundingBox.min = modelMatrix * objects.begin()->second.matrix * objects.begin()->second.model->data[0].pos;
		modelBoundingBox.max = modelBoundingBox.min;

		map<string, Object>::iterator it;
		for (it=objects.begin(); it!=objects.end(); ++it)
		{
			Matrix mat = modelMatrix * it->second.matrix;
			int i;
			for (i=0; i<it->second.model->vertexCount; i++)
			{
				Vector pos = mat * it->second.model->data[i].pos;
				int j;
				for (j=0; j<3; j++)
				{
					if (pos[j] < modelBoundingBox.min[j]) modelBoundingBox.min[j] = pos[j];
					if (pos[j] > modelBoundingBox.max[j]) modelBoundingBox.max[j] = pos[j];
				};
			};
		};

		bbDirty = false;
	};

	return modelBoundingBox;
};

Entity* Entity::checkCollision()
{
	vector<Entity*>::iterator it;
	for (it=World::entities.begin(); it!=World::entities.end(); ++it)
	{
		if ((*it) != this)
		{
			if (CollisionCheck::Entities(this, *it))
			{
				return *it;
			};
		};
	};

	return NULL;
};

Entity* Entity::move(Vector vec)
{
	translate(vec);
	Entity *coll = checkCollision();
	if (coll != NULL)
	{
		translate(-vec);
		return coll;
	};
	return NULL;
};
<|endoftext|>