<commit_before>#include "NodeSSPI.h"

using namespace v8;
using namespace std;

sspi_module_rec sspiModuleInfo = { 0, };

std::map<std::string, credHandleRec> credMap;

void sspi_module_cleanup()
{
	if (sspiModuleInfo.securityDLL != NULL) {
		if (sspiModuleInfo.functable != NULL) {
			sspiModuleInfo.functable->FreeContextBuffer(sspiModuleInfo.pkgInfo);
		}
		FreeLibrary(sspiModuleInfo.securityDLL);
	}
}

void init_module()
{
	LPSTR lpDllName = WINNT_SECURITY_DLL;
	INIT_SECURITY_INTERFACE pInit;
	SECURITY_STATUS ss = SEC_E_INTERNAL_ERROR;

	sspiModuleInfo.defaultPackage = DEFAULT_SSPI_PACKAGE;
	__try {
		sspiModuleInfo.securityDLL = LoadLibrary(lpDllName);
		pInit = (INIT_SECURITY_INTERFACE)GetProcAddress(sspiModuleInfo.securityDLL, SECURITY_ENTRYPOINT);
		sspiModuleInfo.functable = pInit();
		ss = sspiModuleInfo.functable->EnumerateSecurityPackages(&sspiModuleInfo.numPackages, &sspiModuleInfo.pkgInfo);
		sspiModuleInfo.supportsSSPI = TRUE;
	}
	__finally {
		if (ss != SEC_E_OK) {
			sspi_module_cleanup();
		}
	}
}

void note_sspi_auth_failure(const Local<Object> opts,const Local<Object> req,Local<Object> res){
	int nWays = 0;
	int nSSPIPkgs = 0;
	bool offerBasic = false, offerSSPI = false;
	if(opts->Get(String::New("offerBasic"))->BooleanValue()){
		offerBasic = true;
		nWays += 1;
	}
	if(opts->Get(String::New("offerSSPI"))->BooleanValue()){
		offerSSPI = true;
		nSSPIPkgs = opts->Get(String::New("sspiPackagesUsed"))->ToObject()->Get(String::New("length"))->ToInteger()->Uint32Value();
		nWays += nSSPIPkgs;
	}
	auto authHArr = v8::Array::New(nWays);
	int curIdx = 0;
	if(offerBasic){
		std::string basicStr("Basic");
		if(opts->Has(String::New("domain"))){
			basicStr += " realm=\"";
			basicStr += std::string(*String::AsciiValue(opts->Get(String::New("domain"))));
			basicStr += "\"";
		}
		authHArr->Set(curIdx++, String::New(basicStr.c_str()));
	}
	if(offerSSPI){
		for(int i =0;i<nSSPIPkgs;i++){
			authHArr->Set(curIdx++,opts->Get(String::New("sspiPackagesUsed"))->ToObject()->Get(i));
		}
	}
	Handle<Value> argv[] = { String::New("WWW-Authenticate"), authHArr };
	res->Get(String::New("setHeader"))->ToObject()->CallAsFunction(res, 2, argv);
	res->Set(String::New("statusCode"),Integer::New(401));
}

void cleanup_sspi_connection(Local<Object> conn)
{
	sspi_connection_rec *pSCR = 0;
	PCtxtHandle outPch = 0;
	if (conn->HasOwnProperty(String::New("svrCtx"))){
		Local<External> wrap = Local<External>::Cast(conn->Get(String::New("svrCtx"))->ToObject()->GetInternalField(0));
		pSCR = static_cast<sspi_connection_rec *>(wrap->Value());
		outPch = &pSCR->server_context;
		sspiModuleInfo.functable->DeleteSecurityContext(outPch);
		outPch->dwLower = outPch->dwUpper = 0;
		free(pSCR);
		conn->Delete(String::New("svrCtx"));
	}
}

/*
* args[0]: opts
* args[1]: req
* args[2]: res
*/
Handle<Value> Authenticate(const Arguments& args) {
	HandleScope scope;
	BYTE* pToken;
	try{
		if (sspiModuleInfo.supportsSSPI == FALSE) {
			throw std::exception("Doesn't suport SSPI.");
		}
		auto opts = args[0]->ToObject();
		auto req = args[1]->ToObject();
		auto res = args[2]->ToObject();
		auto headers = req->Get(String::New("headers"))->ToObject();
		auto conn = req->Get(String::New("connection"))->ToObject();
		if(conn->HasOwnProperty(String::New("user"))){
			return scope.Close(Undefined());
		}
		if(!headers->Has(String::New("authorization"))){
			note_sspi_auth_failure(opts,req,res);
			return scope.Close(Undefined());
		}
		auto aut = std::string(*String::AsciiValue(headers->Get(String::New("authorization"))));
		stringstream ssin(aut);
		std::string schema, strToken;
		ssin >> schema;
		ssin >> strToken;
		// base64 decode strToken
		unique_ptr<BYTE[]> pToken(new BYTE[strToken.length()]);
		int sz = strToken.length();
		if (!Base64Decode(strToken.c_str(), strToken.length(), pToken.get(), &sz)){
			throw std::exception("Cannot decode authorization field.");
		};
		// get max token size defined by SSPI package
		ULONG maxTokSz, i;
		for (i = 0; i < sspiModuleInfo.numPackages; i++){
			if (!schema.compare(sspiModuleInfo.pkgInfo[i].Name)){
				maxTokSz = sspiModuleInfo.pkgInfo[i].cbMaxToken;
				break;
			}
		}
		if (i == sspiModuleInfo.numPackages){
			throw std::exception(("No " + schema + " SSPI package.").c_str());
		}
		// acquire server credential
		if (credMap.find(schema) == credMap.end()){
			credHandleRec temp = { 0, 0 };
			credMap[schema] = temp;
		}
		FILETIME ft;
		SYSTEMTIME st;
		GetSystemTime(&st); // gets current time
		SystemTimeToFileTime(&st, &ft); // converts to file time format
		if (CompareFileTime(&ft, (FILETIME *)(&credMap[schema].exp)) > 0){
			sspiModuleInfo.functable->FreeCredentialsHandle(&credMap[schema].credHandl);
			// cred expired, re-generate
			if (sspiModuleInfo.functable->AcquireCredentialsHandle(
				NULL //pszPrincipal
				, (char*)(schema.c_str()) //pszPackage
				, SECPKG_CRED_INBOUND //fCredentialUse
				, NULL // pvLogonID
				, NULL //pAuthData
				, NULL //pGetKeyFn
				, NULL //pvGetKeyArgument
				, &credMap[schema].credHandl //phCredential
				, &credMap[schema].exp //ptsExpiry
				) != SEC_E_OK){
					throw std::exception("Cannot get server credential");
			}

		}
		// acquire server context from request.connection
		sspi_connection_rec *pSCR = 0;
		PCtxtHandle inPch = 0, outPch = 0;
		PTimeStamp pTS;
		if (conn->HasOwnProperty(String::New("svrCtx"))){
			// this is not initial request
			Local<External> wrap = Local<External>::Cast(conn->Get(String::New("svrCtx"))->ToObject()->GetInternalField(0));
			pSCR = static_cast<sspi_connection_rec *>(wrap->Value());
			inPch = outPch = &pSCR->server_context;
		}
		else{
			pSCR = static_cast<sspi_connection_rec *>(malloc(sizeof(sspi_connection_rec)));
			outPch = &pSCR->server_context;
			Handle<ObjectTemplate> svrCtx_templ = ObjectTemplate::New();
			svrCtx_templ->SetInternalFieldCount(1);
			Local<Object> obj = svrCtx_templ->NewInstance();
			obj->SetInternalField(0, External::New(outPch));
			conn->Set(String::New("svrCtx"), obj);
		}
		pTS = &pSCR->server_ctxtexpiry;
		// call AcceptSecurityContext
		SecBuffer inbuf, outbuf;
		SecBufferDesc inbufdesc, outbufdesc;
		outbuf.cbBuffer = maxTokSz;
		outbuf.BufferType = SECBUFFER_TOKEN;
		unique_ptr<BYTE[]> pOutBuf(new BYTE[maxTokSz]);
		outbuf.pvBuffer = pOutBuf.get();
		outbufdesc.ulVersion = SECBUFFER_VERSION;
		outbufdesc.cBuffers = 1;
		outbufdesc.pBuffers = &outbuf;

		inbuf.BufferType = SECBUFFER_TOKEN;
		inbuf.cbBuffer = sz;
		inbuf.pvBuffer = pToken.get();
		inbufdesc.cBuffers = 1;
		inbufdesc.ulVersion = SECBUFFER_VERSION;
		inbufdesc.pBuffers = &inbuf;
		ULONG ContextAttributes;

		SECURITY_STATUS ss;
		ss = sspiModuleInfo.functable->AcceptSecurityContext(
			&credMap[schema].credHandl	//  _In_opt_     PCredHandle phCredential,
			, inPch //  _Inout_opt_  PCtxtHandle phContext,
			, &inbufdesc //  _In_opt_     PSecBufferDesc pInput,
			, ASC_REQ_DELEGATE //  _In_         ULONG fContextReq,
			, SECURITY_NATIVE_DREP //  _In_         ULONG TargetDataRep,
			, outPch //  _Inout_opt_  PCtxtHandle phNewContext,
			, &outbufdesc //  _Inout_opt_  PSecBufferDesc pOutput,
			, &ContextAttributes //  _Out_        PULONG pfContextAttr,
			, pTS //  _Out_opt_    PTimeStamp ptsTimeStamp
			);
		if (ss == SEC_I_COMPLETE_NEEDED || ss == SEC_I_COMPLETE_AND_CONTINUE) {
			sspiModuleInfo.functable->CompleteAuthToken(outPch, &outbufdesc);
		}

		switch (ss) {
		case SEC_I_COMPLETE_NEEDED:
		case SEC_I_CONTINUE_NEEDED:
		case SEC_I_COMPLETE_AND_CONTINUE:
			{
				CStringA base64;
				int base64Length = Base64EncodeGetRequiredLength(outbuf.cbBuffer);
				Base64Encode(pOutBuf.get(),
					outbuf.cbBuffer,
					base64.GetBufferSetLength(base64Length),
					&base64Length, ATL_BASE64_FLAG_NOCRLF);
				base64.ReleaseBufferSetLength(base64Length);
				std::string authHStr = schema + " " + std::string(base64.GetString());
				Handle<Value> argv[] = { String::New("WWW-Authenticate"), String::New(authHStr.c_str()) };
				res->Get(String::New("setHeader"))->ToObject()->CallAsFunction(res, 2, argv);
				res->Set(String::New("statusCode"), Integer::New(401));
				break;
			}
		case SEC_E_INVALID_TOKEN:
		case SEC_E_LOGON_DENIED:
			{
				note_sspi_auth_failure(opts,req,res);
				cleanup_sspi_connection(conn);
				res->Set(String::New("statusCode"), Integer::New(401));
				break;
			}
		case SEC_E_INVALID_HANDLE:
		case SEC_E_INTERNAL_ERROR:
		case SEC_E_NO_AUTHENTICATING_AUTHORITY:
		case SEC_E_INSUFFICIENT_MEMORY:
			{
				cleanup_sspi_connection(conn);
				res->Set(String::New("statusCode"), Integer::New(500));
				break;
			}
		case SEC_E_OK:
			//return OK;
			break;
		}
	}
	catch (std::exception& ex){
		args[2]->ToObject()->Set(String::New("statusCode"), Integer::New(500));
		// throw exception to js land
		return v8::ThrowException(v8::String::New(ex.what()));
	}
	return scope.Close(Undefined());
}

void init(Handle<Object> exports) {
	init_module();
	exports->Set(String::NewSymbol("authenticate"),
		FunctionTemplate::New(Authenticate)->GetFunction());
}

NODE_MODULE(nodeSSPI, init)<commit_msg>retrieve user name!<commit_after>#include "NodeSSPI.h"

using namespace v8;
using namespace std;

sspi_module_rec sspiModuleInfo = { 0, };

std::map<std::string, credHandleRec> credMap;

void sspi_module_cleanup()
{
	if (sspiModuleInfo.securityDLL != NULL) {
		if (sspiModuleInfo.functable != NULL) {
			sspiModuleInfo.functable->FreeContextBuffer(sspiModuleInfo.pkgInfo);
		}
		FreeLibrary(sspiModuleInfo.securityDLL);
	}
}

void init_module()
{
	LPSTR lpDllName = WINNT_SECURITY_DLL;
	INIT_SECURITY_INTERFACE pInit;
	SECURITY_STATUS ss = SEC_E_INTERNAL_ERROR;

	sspiModuleInfo.defaultPackage = DEFAULT_SSPI_PACKAGE;
	__try {
		sspiModuleInfo.securityDLL = LoadLibrary(lpDllName);
		pInit = (INIT_SECURITY_INTERFACE)GetProcAddress(sspiModuleInfo.securityDLL, SECURITY_ENTRYPOINT);
		sspiModuleInfo.functable = pInit();
		ss = sspiModuleInfo.functable->EnumerateSecurityPackages(&sspiModuleInfo.numPackages, &sspiModuleInfo.pkgInfo);
		sspiModuleInfo.supportsSSPI = TRUE;
	}
	__finally {
		if (ss != SEC_E_OK) {
			sspi_module_cleanup();
		}
	}
}

void note_sspi_auth_failure(const Local<Object> opts,const Local<Object> req,Local<Object> res){
	int nWays = 0;
	int nSSPIPkgs = 0;
	bool offerBasic = false, offerSSPI = false;
	if(opts->Get(String::New("offerBasic"))->BooleanValue()){
		offerBasic = true;
		nWays += 1;
	}
	if(opts->Get(String::New("offerSSPI"))->BooleanValue()){
		offerSSPI = true;
		nSSPIPkgs = opts->Get(String::New("sspiPackagesUsed"))->ToObject()->Get(String::New("length"))->ToInteger()->Uint32Value();
		nWays += nSSPIPkgs;
	}
	auto authHArr = v8::Array::New(nWays);
	int curIdx = 0;
	if(offerBasic){
		std::string basicStr("Basic");
		if(opts->Has(String::New("domain"))){
			basicStr += " realm=\"";
			basicStr += std::string(*String::AsciiValue(opts->Get(String::New("domain"))));
			basicStr += "\"";
		}
		authHArr->Set(curIdx++, String::New(basicStr.c_str()));
	}
	if(offerSSPI){
		for(int i =0;i<nSSPIPkgs;i++){
			authHArr->Set(curIdx++,opts->Get(String::New("sspiPackagesUsed"))->ToObject()->Get(i));
		}
	}
	Handle<Value> argv[] = { String::New("WWW-Authenticate"), authHArr };
	res->Get(String::New("setHeader"))->ToObject()->CallAsFunction(res, 2, argv);
	res->Set(String::New("statusCode"),Integer::New(401));
}

void cleanup_sspi_connection(Local<Object> conn)
{
	sspi_connection_rec *pSCR = 0;
	PCtxtHandle outPch = 0;
	if (conn->HasOwnProperty(String::New("svrCtx"))){
		Local<External> wrap = Local<External>::Cast(conn->Get(String::New("svrCtx"))->ToObject()->GetInternalField(0));
		pSCR = static_cast<sspi_connection_rec *>(wrap->Value());
		outPch = &pSCR->server_context;
		sspiModuleInfo.functable->DeleteSecurityContext(outPch);
		outPch->dwLower = outPch->dwUpper = 0;
		free(pSCR);
		conn->Delete(String::New("svrCtx"));
	}
}

/*
* args[0]: opts
* args[1]: req
* args[2]: res
*/
Handle<Value> Authenticate(const Arguments& args) {
	HandleScope scope;
	BYTE* pToken;
	try{
		if (sspiModuleInfo.supportsSSPI == FALSE) {
			throw std::exception("Doesn't suport SSPI.");
		}
		auto opts = args[0]->ToObject();
		auto req = args[1]->ToObject();
		auto res = args[2]->ToObject();
		auto headers = req->Get(String::New("headers"))->ToObject();
		auto conn = req->Get(String::New("connection"))->ToObject();
		if(conn->HasOwnProperty(String::New("user"))){
			return scope.Close(Undefined());
		}
		if(!headers->Has(String::New("authorization"))){
			note_sspi_auth_failure(opts,req,res);
			return scope.Close(Undefined());
		}
		auto aut = std::string(*String::AsciiValue(headers->Get(String::New("authorization"))));
		stringstream ssin(aut);
		std::string schema, strToken;
		ssin >> schema;
		ssin >> strToken;
		// base64 decode strToken
		unique_ptr<BYTE[]> pToken(new BYTE[strToken.length()]);
		int sz = strToken.length();
		if (!Base64Decode(strToken.c_str(), strToken.length(), pToken.get(), &sz)){
			throw std::exception("Cannot decode authorization field.");
		};
		// get max token size defined by SSPI package
		ULONG maxTokSz, i;
		for (i = 0; i < sspiModuleInfo.numPackages; i++){
			if (!schema.compare(sspiModuleInfo.pkgInfo[i].Name)){
				maxTokSz = sspiModuleInfo.pkgInfo[i].cbMaxToken;
				break;
			}
		}
		if (i == sspiModuleInfo.numPackages){
			throw std::exception(("No " + schema + " SSPI package.").c_str());
		}
		// acquire server credential
		if (credMap.find(schema) == credMap.end()){
			credHandleRec temp = { 0, 0 };
			credMap[schema] = temp;
		}
		FILETIME ft;
		SYSTEMTIME st;
		GetSystemTime(&st); // gets current time
		SystemTimeToFileTime(&st, &ft); // converts to file time format
		if (CompareFileTime(&ft, (FILETIME *)(&credMap[schema].exp)) > 0){
			sspiModuleInfo.functable->FreeCredentialsHandle(&credMap[schema].credHandl);
			// cred expired, re-generate
			if (sspiModuleInfo.functable->AcquireCredentialsHandle(
				NULL //pszPrincipal
				, (char*)(schema.c_str()) //pszPackage
				, SECPKG_CRED_INBOUND //fCredentialUse
				, NULL // pvLogonID
				, NULL //pAuthData
				, NULL //pGetKeyFn
				, NULL //pvGetKeyArgument
				, &credMap[schema].credHandl //phCredential
				, &credMap[schema].exp //ptsExpiry
				) != SEC_E_OK){
					throw std::exception("Cannot get server credential");
			}

		}
		// acquire server context from request.connection
		sspi_connection_rec *pSCR = 0;
		PCtxtHandle inPch = 0, outPch = 0;
		PTimeStamp pTS;
		if (conn->HasOwnProperty(String::New("svrCtx"))){
			// this is not initial request
			Local<External> wrap = Local<External>::Cast(conn->Get(String::New("svrCtx"))->ToObject()->GetInternalField(0));
			pSCR = static_cast<sspi_connection_rec *>(wrap->Value());
			inPch = outPch = &pSCR->server_context;
		}
		else{
			pSCR = static_cast<sspi_connection_rec *>(malloc(sizeof(sspi_connection_rec)));
			outPch = &pSCR->server_context;
			Handle<ObjectTemplate> svrCtx_templ = ObjectTemplate::New();
			svrCtx_templ->SetInternalFieldCount(1);
			Local<Object> obj = svrCtx_templ->NewInstance();
			obj->SetInternalField(0, External::New(outPch));
			conn->Set(String::New("svrCtx"), obj);
		}
		pTS = &pSCR->server_ctxtexpiry;
		// call AcceptSecurityContext
		SecBuffer inbuf, outbuf;
		SecBufferDesc inbufdesc, outbufdesc;
		outbuf.cbBuffer = maxTokSz;
		outbuf.BufferType = SECBUFFER_TOKEN;
		unique_ptr<BYTE[]> pOutBuf(new BYTE[maxTokSz]);
		outbuf.pvBuffer = pOutBuf.get();
		outbufdesc.ulVersion = SECBUFFER_VERSION;
		outbufdesc.cBuffers = 1;
		outbufdesc.pBuffers = &outbuf;

		inbuf.BufferType = SECBUFFER_TOKEN;
		inbuf.cbBuffer = sz;
		inbuf.pvBuffer = pToken.get();
		inbufdesc.cBuffers = 1;
		inbufdesc.ulVersion = SECBUFFER_VERSION;
		inbufdesc.pBuffers = &inbuf;
		ULONG ContextAttributes;

		SECURITY_STATUS ss;
		ss = sspiModuleInfo.functable->AcceptSecurityContext(
			&credMap[schema].credHandl	//  _In_opt_     PCredHandle phCredential,
			, inPch //  _Inout_opt_  PCtxtHandle phContext,
			, &inbufdesc //  _In_opt_     PSecBufferDesc pInput,
			, ASC_REQ_DELEGATE //  _In_         ULONG fContextReq,
			, SECURITY_NATIVE_DREP //  _In_         ULONG TargetDataRep,
			, outPch //  _Inout_opt_  PCtxtHandle phNewContext,
			, &outbufdesc //  _Inout_opt_  PSecBufferDesc pOutput,
			, &ContextAttributes //  _Out_        PULONG pfContextAttr,
			, pTS //  _Out_opt_    PTimeStamp ptsTimeStamp
			);
		if (ss == SEC_I_COMPLETE_NEEDED || ss == SEC_I_COMPLETE_AND_CONTINUE) {
			sspiModuleInfo.functable->CompleteAuthToken(outPch, &outbufdesc);
		}

		switch (ss) {
		case SEC_I_COMPLETE_NEEDED:
		case SEC_I_CONTINUE_NEEDED:
		case SEC_I_COMPLETE_AND_CONTINUE:
			{
				CStringA base64;
				int base64Length = Base64EncodeGetRequiredLength(outbuf.cbBuffer);
				Base64Encode(pOutBuf.get(),
					outbuf.cbBuffer,
					base64.GetBufferSetLength(base64Length),
					&base64Length, ATL_BASE64_FLAG_NOCRLF);
				base64.ReleaseBufferSetLength(base64Length);
				std::string authHStr = schema + " " + std::string(base64.GetString());
				Handle<Value> argv[] = { String::New("WWW-Authenticate"), String::New(authHStr.c_str()) };
				res->Get(String::New("setHeader"))->ToObject()->CallAsFunction(res, 2, argv);
				res->Set(String::New("statusCode"), Integer::New(401));
				break;
			}
		case SEC_E_INVALID_TOKEN:
		case SEC_E_LOGON_DENIED:
			{
				note_sspi_auth_failure(opts,req,res);
				cleanup_sspi_connection(conn);
				res->Set(String::New("statusCode"), Integer::New(401));
				break;
			}
		case SEC_E_INVALID_HANDLE:
		case SEC_E_INTERNAL_ERROR:
		case SEC_E_NO_AUTHENTICATING_AUTHORITY:
		case SEC_E_INSUFFICIENT_MEMORY:
			{
				cleanup_sspi_connection(conn);
				res->Set(String::New("statusCode"), Integer::New(500));
				break;
			}
		case SEC_E_OK:
			{
				// get user name
				SecPkgContext_Names names;
				SECURITY_STATUS ss;
				char *retval = NULL;

				if ((ss = sspiModuleInfo.functable->QueryContextAttributes(outPch,
					SECPKG_ATTR_NAMES,
					&names)
					) == SEC_E_OK) {
						conn->Set(String::New("user"),String::New(names.sUserName));
						sspiModuleInfo.functable->FreeContextBuffer(names.sUserName);
				}
				break;
			}
		}
	}
	catch (std::exception& ex){
		args[2]->ToObject()->Set(String::New("statusCode"), Integer::New(500));
		// throw exception to js land
		return v8::ThrowException(v8::String::New(ex.what()));
	}
	return scope.Close(Undefined());
}

void init(Handle<Object> exports) {
	init_module();
	exports->Set(String::NewSymbol("authenticate"),
		FunctionTemplate::New(Authenticate)->GetFunction());
}

NODE_MODULE(nodeSSPI, init)<|endoftext|>


<commit_before>#include "ObjPlane.h"

/*
====================
Plane::Plane
	Takes in the input stream and creates a Plane object from the parsed input
====================
*/
Plane::Plane(ifstream &f) {
	string textureName;

	while(!f.eof()) {
		string line;
		getline(f,line);

		queue<string> lineContents;
		explode(line," ",&lineContents);

		if(lineContents.size() == 0) continue;
		string word = lineContents.front();
		lineContents.pop();

		if(word[0] == '#' || line[0] == '\n' || line[0] == '\r') continue;

		//words with three arguments
		if(word == "origin" || word == "color" || word == "normal" || word == "up") {
			double num1 = 0;
			double num2 = 0;
			double num3 = 0;

			if(lineContents.size() < 3) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num2 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num3 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			if(word == "origin")		this->origin = Point(num1,num2,num3);
			else if(word == "color")	material.color = Color(num1,num2,num3);
			else if(word == "up") {
				this->up = Vector(num1,num2,num3);
			}
			else if(word == "normal") {
				this->normal = Vector(num1,num2,num3);
			}
		}

		//words with two arguments
		else if(word == "size") {
			double num1 = 0;
			double num2 = 0;

			if(lineContents.size() < 2) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num2 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			this->width = num1;
			this->height = num2;
		}

		//words with one double argument
		else if(word == "reflect" || word == "transparency") {
			double num1 = 0;

			if(lineContents.size() < 1) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			if(word == "reflect")			this->material.reflection = num1;
			else if (word == "transparency")this->material.transparency = num1;
		}

		//words with one string argument
		else if(word == "texture") {
			//TODO: attempt load texture from local directory first
			if(lineContents.size() < 1) break;

			textureName = lineContents.front();
			lineContents.pop();
		}
		else break;
	}

	sharedInit(material,width,height,up,normal,textureName,origin);
}

/*
====================
Plane::Plane
	Takes information for creation from parameters instead of input stream
	so that it can be created by a complex object
====================
*/
Plane::Plane(Material m, double width, double height, Vector up, Vector normal, string textureName, Point origin) {
	sharedInit(m,width,height,up,normal,textureName,origin);
}

/*
====================
Plane::sharedInit
	Shared initialization method for all Plane objects
====================
*/
void Plane::sharedInit(Material m, double width, double height, Vector up, Vector normal, string textureName, Point origin) {
	//Warns if Vectors are not orthogonal
	if(abs(up.dot(normal)) > 0.00001) {
		printf("Warning: Plane up vector ");
		up.print();
		printf(" and normal vector ");
		normal.print();
		printf(" are not orthogonal.\n");
	}

	this->objectType = ENTITY_PLANE;

	this->origin = origin;
	this->width = width;
	this->height = height;
	this->material = m;
	this->up = up;
	this->normal = normal;
	this->hasTexture = false;
	if(textureName.size() > 0 && this->texture.ReadFromFile(textureName.c_str())) {
		this->hasTexture = true;
	}
	this->isLight = false;
	this->isVisible = true;

	this->up.normalize();
	this->normal.normalize();

	Vector left = this->normal.cross(this->up);
	Point upMid = (this->up * this->height * 0.5) + this->origin;
	vertex1 = upMid + (left * this->width * 0.5);
	vertex2 = upMid - (left * this->width * 0.5);
	vertex3 = vertex1 - (this->up * this->height);
	vertex4 = vertex2 - (this->up * this->height);
}

/*
====================
Plane::intersect
	Computes intersection between the Plane and the ray, and returns itself if
	it is hit or NULL if it is not along with the point of intersection
====================
*/
SceneObject* Plane::intersect(Ray* r, Point &intersect) {
	double dot = normal.dot(r->dir);

	if(dot != 0) { //If the normal and ray aren't perpendicular (ray and plane parallel)
		double t = normal.dot(origin - r->start) / dot;
		if(t >= 0) { //If the ray is pointing toward the plane
			//Calculate point of intersection on plane
			Point tempInt = r->dir * t + r->start;

			//Calculate vectors that define the rectangle and vector to point
			// +----------> topLine
			// |\
			// | \ topLeftToPoint
			// v
			// leftLine
			Vector topLine =  vertex2 - vertex1;
			Vector leftLine = vertex3 - vertex1;
			Vector topLeftToPoint = tempInt - vertex1;

			double leftProjectionLength	= leftLine.dot(topLeftToPoint);
			double leftLineLength		= leftLine.dot(leftLine);
			double topProjectionLength	= topLine.dot(topLeftToPoint);
			double topLineLength		= topLine.dot(topLine);

			if(0 <= leftProjectionLength && leftProjectionLength <= leftLineLength &&
			   0 <= topProjectionLength  && topProjectionLength  <= topLineLength) {
				intersect = tempInt;
				return this;
			}
		}
	}
	return NULL;
}

/*
====================
calculateTextureFromMaterial
	based on the point of interception, calculates what color from the texture
	should be returned
====================
*/
Color Plane::calculateTextureFromMaterial(Point intercept) {
	BMP* temp;
	temp = &this->texture;
	int height = temp->TellHeight();
	int width = temp->TellWidth();
	double planeHeight = this->height;
	double pixelSize = height / planeHeight; //Width and height of pixel on plane

	//Figure out the boundaries of the plane
	Vector left = normal.cross(up);

	Point upMid = (this->up * this->height * 0.5) + this->origin;
	Point upLeft = upMid + (left * this->width * 0.5);
	Point upRight = upMid - (left * this->width * 0.5);
	Point botLeft = upLeft - (this->up * this->height);
	Point botRight = upRight - (this->up * this->height);

	double heightPercentage = 0;
	double widthPercentage = 0;

	Ray top;
	top.dir = upLeft - upRight;
	top.start = upRight;

	Ray planeUp;
	planeUp.dir = this->up;
	planeUp.start = intercept;

	// ((p2 - p1)xd1) . (d1xd2) / ||d1xd2||^2
	//percentage along top of plane
	double tTopPoint = (planeUp.start - top.start).cross(planeUp.dir).dot(top.dir.cross(planeUp.dir)) / top.dir.cross(planeUp.dir).magnitude2();
	//double tTopPoint = dot3((planeUp.start - top.start).cross(planeUp.dir), top.dir.cross(planeUp.dir))/magnitude2(top.dir.cross(planeUp.dir));

	Ray side;
	side.dir = upLeft - botLeft;
	side.start = botLeft;
	side.dir.normalize();

	Ray planeLeft;
	planeLeft.dir = left;
	planeLeft.start = intercept;

	//percentage along side of plane
	//double tLeftPoint = 1 - (1.f/this->height) * (planeLeft.start - side.start).cross(planeLeft.dir).dot(side.dir.cross(planeLeft.dir)) / magnitude2(side.dir.cross(planeLeft.dir));
	//double tLeftPoint = 1 - (1.f/this->height) * dot3((planeLeft.start - side.start).cross(planeLeft.dir),side.dir.cross(planeLeft.dir))/magnitude2(side.dir.cross(planeLeft.dir));
	double tLeftPoint = 1 - (1.f/this->height) * (planeLeft.start - side.start).cross(planeLeft.dir).dot(side.dir.cross(planeLeft.dir)) / side.dir.cross(planeLeft.dir).magnitude2();

	int pixelX = abs((int)(tTopPoint * width));
	int pixelY = abs((int)(tLeftPoint * height));
	pixelX %= width;
	pixelY %= height;
	Color matColor;
	if(DIAGNOSTIC_STATUS == DIAGNOSTIC_TEXTURE_MAPPING) {
		matColor = Color(tLeftPoint,1,tTopPoint);
	}
	else {
		matColor.r = temp->GetPixel(pixelX,pixelY).Red/255.f;
		matColor.g = temp->GetPixel(pixelX,pixelY).Green/255.f;
		matColor.b = temp->GetPixel(pixelX,pixelY).Blue/255.f;
	}

	return matColor;
}

/*
====================
Plane::calculateNormalForPoint
	Calculates the normal of the point p on the object as hit from ray r.
	I assume an infinitely wide Plane, so this will always return the normal
	as if the ray hit the Plane
====================
*/
Vector Plane::calculateNormalForPoint(Point p, Point raySource) {
	double distNormalSide = (p + this->normal).comparitiveDistanceFrom(raySource);
	double distOtherSide =	(p - this->normal).comparitiveDistanceFrom(raySource);
	if(distOtherSide < distNormalSide) return this->normal * -1;
	return this->normal;
}

/*
====================
Plane::getReflection
	Returns the reflection coefficient
====================
*/
double Plane::getReflection() {
	return this->material.reflection;
}

/*
====================
Plane::getRefraction
	Returns the refraction coefficient
====================
*/
double Plane::getRefraction() {
	return this->material.transparency;
}

/*
====================
Plane::getColor
	Returns the color of the object (ignoring texture color)
====================
*/
Color Plane::getColor() {
	return this->material.color;
}
<commit_msg>Simplified plane texture projection.<commit_after>#include "ObjPlane.h"

/*
====================
Plane::Plane
	Takes in the input stream and creates a Plane object from the parsed input
====================
*/
Plane::Plane(ifstream &f) {
	string textureName;

	while(!f.eof()) {
		string line;
		getline(f,line);

		queue<string> lineContents;
		explode(line," ",&lineContents);

		if(lineContents.size() == 0) continue;
		string word = lineContents.front();
		lineContents.pop();

		if(word[0] == '#' || line[0] == '\n' || line[0] == '\r') continue;

		//words with three arguments
		if(word == "origin" || word == "color" || word == "normal" || word == "up") {
			double num1 = 0;
			double num2 = 0;
			double num3 = 0;

			if(lineContents.size() < 3) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num2 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num3 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			if(word == "origin")		this->origin = Point(num1,num2,num3);
			else if(word == "color")	material.color = Color(num1,num2,num3);
			else if(word == "up") {
				this->up = Vector(num1,num2,num3);
			}
			else if(word == "normal") {
				this->normal = Vector(num1,num2,num3);
			}
		}

		//words with two arguments
		else if(word == "size") {
			double num1 = 0;
			double num2 = 0;

			if(lineContents.size() < 2) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();
			num2 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			this->width = num1;
			this->height = num2;
		}

		//words with one double argument
		else if(word == "reflect" || word == "transparency") {
			double num1 = 0;

			if(lineContents.size() < 1) break;

			num1 = (double)atof(lineContents.front().c_str());
			lineContents.pop();

			if(word == "reflect")			this->material.reflection = num1;
			else if (word == "transparency")this->material.transparency = num1;
		}

		//words with one string argument
		else if(word == "texture") {
			//TODO: attempt load texture from local directory first
			if(lineContents.size() < 1) break;

			textureName = lineContents.front();
			lineContents.pop();
		}
		else break;
	}

	sharedInit(material,width,height,up,normal,textureName,origin);
}

/*
====================
Plane::Plane
	Takes information for creation from parameters instead of input stream
	so that it can be created by a complex object
====================
*/
Plane::Plane(Material m, double width, double height, Vector up, Vector normal, string textureName, Point origin) {
	sharedInit(m,width,height,up,normal,textureName,origin);
}

/*
====================
Plane::sharedInit
	Shared initialization method for all Plane objects
====================
*/
void Plane::sharedInit(Material m, double width, double height, Vector up, Vector normal, string textureName, Point origin) {
	//Warns if Vectors are not orthogonal
	if(abs(up.dot(normal)) > 0.00001) {
		printf("Warning: Plane up vector ");
		up.print();
		printf(" and normal vector ");
		normal.print();
		printf(" are not orthogonal.\n");
	}

	this->objectType = ENTITY_PLANE;

	this->origin = origin;
	this->width = width;
	this->height = height;
	this->material = m;
	this->up = up;
	this->normal = normal;
	this->hasTexture = false;
	if(textureName.size() > 0 && this->texture.ReadFromFile(textureName.c_str())) {
		this->hasTexture = true;
	}
	this->isLight = false;
	this->isVisible = true;

	this->up.normalize();
	this->normal.normalize();

	Vector left = this->normal.cross(this->up);
	Point upMid = (this->up * this->height * 0.5) + this->origin;
	vertex1 = upMid + (left * this->width * 0.5);
	vertex2 = upMid - (left * this->width * 0.5);
	vertex3 = vertex1 - (this->up * this->height);
	vertex4 = vertex2 - (this->up * this->height);
}

/*
====================
Plane::intersect
	Computes intersection between the Plane and the ray, and returns itself if
	it is hit or NULL if it is not along with the point of intersection
====================
*/
SceneObject* Plane::intersect(Ray* r, Point &intersect) {
	double dot = normal.dot(r->dir);

	if(dot != 0) { //If the normal and ray aren't perpendicular (ray and plane parallel)
		double t = normal.dot(origin - r->start) / dot;
		if(t >= 0) { //If the ray is pointing toward the plane
			//Calculate point of intersection on plane
			Point tempInt = r->dir * t + r->start;

			//Calculate vectors that define the rectangle and vector to point
			// +----------> topLine
			// |\
			// | \ topLeftToPoint
			// v
			// leftLine
			Vector topLine =  vertex2 - vertex1;
			Vector leftLine = vertex3 - vertex1;
			Vector topLeftToPoint = tempInt - vertex1;

			double leftProjectionLength	= leftLine.dot(topLeftToPoint);
			double leftLineLength		= leftLine.dot(leftLine);
			double topProjectionLength	= topLine.dot(topLeftToPoint);
			double topLineLength		= topLine.dot(topLine);

			if(0 <= leftProjectionLength && leftProjectionLength <= leftLineLength &&
			   0 <= topProjectionLength  && topProjectionLength  <= topLineLength) {
				intersect = tempInt;
				return this;
			}
		}
	}
	return NULL;
}

/*
====================
calculateTextureFromMaterial
	based on the point of interception, calculates what color from the texture
	should be returned
====================
*/
Color Plane::calculateTextureFromMaterial(Point intercept) {
	BMP* temp;
	temp = &this->texture;
	int height = temp->TellHeight();
	int width = temp->TellWidth();
	double planeHeight = this->height;
	double pixelSize = height / planeHeight; //Width and height of pixel on plane

	Vector topLine =  vertex2 - vertex1;
	Vector leftLine = vertex3 - vertex1;
	Vector topLeftToPoint = intercept - vertex1;

	double leftProjectionLength	= leftLine.dot(topLeftToPoint);
	double leftLineLength		= leftLine.dot(leftLine);
	double topProjectionLength	= topLine.dot(topLeftToPoint);
	double topLineLength		= topLine.dot(topLine);

	double heightPercentage = leftProjectionLength / leftLineLength;
	double widthPercentage  = topProjectionLength / topLineLength;

	int pixelX = abs((int)(widthPercentage * width));
	int pixelY = abs((int)(heightPercentage * height));
	pixelX %= width;
	pixelY %= height;
	Color matColor;
	if(DIAGNOSTIC_STATUS == DIAGNOSTIC_TEXTURE_MAPPING) {
		matColor = Color(heightPercentage,1,widthPercentage);
	}
	else {
		matColor.r = temp->GetPixel(pixelX,pixelY).Red/255.f;
		matColor.g = temp->GetPixel(pixelX,pixelY).Green/255.f;
		matColor.b = temp->GetPixel(pixelX,pixelY).Blue/255.f;
	}

	return matColor;
}

/*
====================
Plane::calculateNormalForPoint
	Calculates the normal of the point p on the object as hit from ray r.
	I assume an infinitely wide Plane, so this will always return the normal
	as if the ray hit the Plane
====================
*/
Vector Plane::calculateNormalForPoint(Point p, Point raySource) {
	double distNormalSide = (p + this->normal).comparitiveDistanceFrom(raySource);
	double distOtherSide =	(p - this->normal).comparitiveDistanceFrom(raySource);
	if(distOtherSide < distNormalSide) return this->normal * -1;
	return this->normal;
}

/*
====================
Plane::getReflection
	Returns the reflection coefficient
====================
*/
double Plane::getReflection() {
	return this->material.reflection;
}

/*
====================
Plane::getRefraction
	Returns the refraction coefficient
====================
*/
double Plane::getRefraction() {
	return this->material.transparency;
}

/*
====================
Plane::getColor
	Returns the color of the object (ignoring texture color)
====================
*/
Color Plane::getColor() {
	return this->material.color;
}
<|endoftext|>


<commit_before>//
//  PTSolver.cpp
//  PowerTools++
//
//  Created by Hassan on 30/01/2015.
//  Copyright (c) 2015 NICTA. All rights reserved.
//

#include "PowerTools++/PTSolver.h"

#ifdef USE_BONMIN
#include <coin/BonBonminSetup.hpp>
#include <coin/BonCbc.hpp>
#endif

namespace {
    void gurobiNotAvailable()
    {
        cerr << "Can't use Gurobi for solver: this version of PowerTools "
        "was compiled without Gurobi support." << endl;
        exit(1);
    }
    void bonminNotAvailable()
    {
        cerr << "Can't use Bonmin for solver: this version of PowerTools "
        "was compiled without Bonmin support." << endl;
        exit(1);
    }
}

PTSolver::PTSolver(Model* model, SolverType stype){
    _stype = stype;
    switch (stype) {
        case ipopt:
            prog.ipopt_prog = new IpoptProgram(model);
            break;
        case gurobi:
#ifdef USE_GUROBI
            try{
                prog.grb_prog = new GurobiProgram(model);
            }catch(GRBException e) {
                cerr << "\nError code = " << e.getErrorCode() << endl;
                cerr << e.getMessage() << endl;
                exit(1);
            }
#else
            gurobiNotAvailable();
#endif
            break;
        case bonmin:
#ifdef USE_BONMIN
            prog.bonmin_prog = new BonminProgram(model);
#else
            bonminNotAvailable();
#endif
            break;
    }
}

PTSolver::~PTSolver(){
    if (_stype == gurobi) {
#ifdef USE_GUROBI
        delete prog.grb_prog;
#else
        gurobiNotAvailable();
#endif
    }
    if (_stype == ipopt) delete prog.ipopt_prog;
}

void PTSolver::set_model(Model* m) {

    if (_stype == gurobi){
#ifdef USE_GUROBI
        prog.grb_prog->model = m;
#else
        gurobiNotAvailable();
#endif
    }
    if (_stype == ipopt) prog.ipopt_prog->model = m;
}

int PTSolver::run(int output, bool relax){
    //GurobiProgram* grbprog;
    // Initialize the IpoptApplication and process the options

    if (_stype==ipopt) {
            IpoptApplication iapp;
            ApplicationReturnStatus status = iapp.Initialize();
            if (status != Solve_Succeeded) {
                std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
                return (int) status;
            }

        if(prog.ipopt_prog->model->_objt==maximize){
            *prog.ipopt_prog->model->_obj *= -1;
        }
        SmartPtr<TNLP> tmp = new IpoptProgram(prog.ipopt_prog->model);
//        prog.ipopt_prog;
            //            iapp.Options()->SetStringValue("hessian_constant", "yes");
//                        iapp.Options()->SetStringValue("derivative_test", "second-order");
            //            iapp->Options()->SetNumericValue("tol", 1e-6);
//                        iapp.Options()->SetNumericValue("tol", 1e-6);
            //            iapp->Options()->SetStringValue("derivative_test", "second-order");
            //            iapp.Options()->SetNumericValue("bound_relax_factor", 0);
            //            iapp.Options()->SetIntegerValue("print_level", 5);

            //            iapp.Options()->SetStringValue("derivative_test_print_all", "yes");
        status = iapp.OptimizeTNLP(tmp);
        if(prog.ipopt_prog->model->_objt==maximize){
            *prog.ipopt_prog->model->_obj *= -1;
        }
            if (status == Solve_Succeeded) {
                // Retrieve some statistics about the solve

                //                printf("\n\nSolution of the primal variables:\n");
                //                _model->print_solution();
//                return status;
                return 100;
            }
        if (status == Solved_To_Acceptable_Level) {
            return 150;
        }
    }
    else if(_stype==gurobi)
    {
#ifdef USE_GUROBI
        try{
            //                prog.grbprog = new GurobiProgram();
            prog.grb_prog->_output = output;
            prog.grb_prog->reset_model();
            prog.grb_prog->prepare_model();
            bool ok = prog.grb_prog->solve(relax);
            return ok ? 100 : -1;
        }catch(GRBException e) {
            cerr << "\nError code = " << e.getErrorCode() << endl;
            cerr << e.getMessage() << endl;
        }
#else
        gurobiNotAvailable();
#endif
    }
    else if(_stype==bonmin) {
#ifdef USE_BONMIN
        using namespace Bonmin;
        BonminSetup bonmin;
        bonmin.initializeOptionsAndJournalist();
        bonmin.initialize(prog.bonmin_prog);
        try {
            Bab bb;
            bb(bonmin);
        }
        catch(TNLPSolver::UnsolvedError *E) {
            //There has been a failure to solve a problem with Bonmin.
            std::cerr<<"Bonmin has failed to solve a problem"<<std::endl;
        }
        catch(OsiTMINLPInterface::SimpleError &E) {
            std::cerr<<E.className()<<"::"<<E.methodName()
            <<std::endl
            <<E.message()<<std::endl;
        }
        catch(CoinError &E) {
            std::cerr<<E.className()<<"::"<<E.methodName()
            <<std::endl
            <<E.message()<<std::endl;
        }
#else
        bonminNotAvailable();
#endif
    }
    return -1;
}
<commit_msg>Allow bonmin to maximise or minimise.<commit_after>//
//  PTSolver.cpp
//  PowerTools++
//
//  Created by Hassan on 30/01/2015.
//  Copyright (c) 2015 NICTA. All rights reserved.
//

#include "PowerTools++/PTSolver.h"

#ifdef USE_BONMIN
#include <coin/BonBonminSetup.hpp>
#include <coin/BonCbc.hpp>
#endif

namespace {
    void gurobiNotAvailable()
    {
        cerr << "Can't use Gurobi for solver: this version of PowerTools "
        "was compiled without Gurobi support." << endl;
        exit(1);
    }
    void bonminNotAvailable()
    {
        cerr << "Can't use Bonmin for solver: this version of PowerTools "
        "was compiled without Bonmin support." << endl;
        exit(1);
    }
}

PTSolver::PTSolver(Model* model, SolverType stype){
    _stype = stype;
    switch (stype) {
        case ipopt:
            prog.ipopt_prog = new IpoptProgram(model);
            break;
        case gurobi:
#ifdef USE_GUROBI
            try{
                prog.grb_prog = new GurobiProgram(model);
            }catch(GRBException e) {
                cerr << "\nError code = " << e.getErrorCode() << endl;
                cerr << e.getMessage() << endl;
                exit(1);
            }
#else
            gurobiNotAvailable();
#endif
            break;
        case bonmin:
#ifdef USE_BONMIN
            prog.bonmin_prog = new BonminProgram(model);
#else
            bonminNotAvailable();
#endif
            break;
    }
}

PTSolver::~PTSolver(){
    if (_stype == gurobi) {
#ifdef USE_GUROBI
        delete prog.grb_prog;
#else
        gurobiNotAvailable();
#endif
    }
    if (_stype == ipopt) delete prog.ipopt_prog;
}

void PTSolver::set_model(Model* m) {

    if (_stype == gurobi){
#ifdef USE_GUROBI
        prog.grb_prog->model = m;
#else
        gurobiNotAvailable();
#endif
    }
    if (_stype == ipopt) prog.ipopt_prog->model = m;
}

int PTSolver::run(int output, bool relax){
    //GurobiProgram* grbprog;
    // Initialize the IpoptApplication and process the options

    if (_stype==ipopt) {
            IpoptApplication iapp;
            ApplicationReturnStatus status = iapp.Initialize();
            if (status != Solve_Succeeded) {
                std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
                return (int) status;
            }

        if(prog.ipopt_prog->model->_objt==maximize){
            *prog.ipopt_prog->model->_obj *= -1;
        }
        SmartPtr<TNLP> tmp = new IpoptProgram(prog.ipopt_prog->model);
//        prog.ipopt_prog;
            //            iapp.Options()->SetStringValue("hessian_constant", "yes");
//                        iapp.Options()->SetStringValue("derivative_test", "second-order");
            //            iapp->Options()->SetNumericValue("tol", 1e-6);
//                        iapp.Options()->SetNumericValue("tol", 1e-6);
            //            iapp->Options()->SetStringValue("derivative_test", "second-order");
            //            iapp.Options()->SetNumericValue("bound_relax_factor", 0);
            //            iapp.Options()->SetIntegerValue("print_level", 5);

            //            iapp.Options()->SetStringValue("derivative_test_print_all", "yes");
        status = iapp.OptimizeTNLP(tmp);
        if(prog.ipopt_prog->model->_objt==maximize){
            *prog.ipopt_prog->model->_obj *= -1;
        }
            if (status == Solve_Succeeded) {
                // Retrieve some statistics about the solve

                //                printf("\n\nSolution of the primal variables:\n");
                //                _model->print_solution();
//                return status;
                return 100;
            }
        if (status == Solved_To_Acceptable_Level) {
            return 150;
        }
    }
    else if(_stype==gurobi)
    {
#ifdef USE_GUROBI
        try{
            //                prog.grbprog = new GurobiProgram();
            prog.grb_prog->_output = output;
            prog.grb_prog->reset_model();
            prog.grb_prog->prepare_model();
            bool ok = prog.grb_prog->solve(relax);
            return ok ? 100 : -1;
        }catch(GRBException e) {
            cerr << "\nError code = " << e.getErrorCode() << endl;
            cerr << e.getMessage() << endl;
        }
#else
        gurobiNotAvailable();
#endif
    }
    else if(_stype==bonmin) {
#ifdef USE_BONMIN
        if(prog.bonmin_prog->model->_objt==maximize){
            *prog.bonmin_prog->model->_obj *= -1;
        }

        bool ok = true;
        using namespace Bonmin;
        BonminSetup bonmin;
        bonmin.initializeOptionsAndJournalist();
        bonmin.initialize(prog.bonmin_prog);
        try {
            Bab bb;
            bb(bonmin);
        }
        catch(TNLPSolver::UnsolvedError *E) {
            //There has been a failure to solve a problem with Bonmin.
            std::cerr<<"Bonmin has failed to solve a problem"<<std::endl;
            ok = false;
        }
        catch(OsiTMINLPInterface::SimpleError &E) {
            std::cerr<<E.className()<<"::"<<E.methodName()
            <<std::endl
            <<E.message()<<std::endl;
            ok = false;
        }
        catch(CoinError &E) {
            std::cerr<<E.className()<<"::"<<E.methodName()
            <<std::endl
            <<E.message()<<std::endl;
            ok = false;
        }

        if(prog.bonmin_prog->model->_objt==maximize){
            *prog.bonmin_prog->model->_obj *= -1;
        }

        return ok ? 100 : -1;
#else
        bonminNotAvailable();
#endif
    }
    return -1;
}
<|endoftext|>


<commit_before>#ifndef _SDD_DD_PROTO_NODE_HH_
#define _SDD_DD_PROTO_NODE_HH_

#include <algorithm>  // equal, for_each
#include <functional> // hash
#include <iosfwd>
#include <vector>

#include "sdd/dd/definition.hh"
#include "sdd/dd/stack.hh"
#include "sdd/util/hash.hh"

namespace sdd {

/*------------------------------------------------------------------------------------------------*/

template <typename C>
struct proto_arc
{
  using values_type = typename C::Values;
  using value_type = typename values_type::value_type;

  using value_stack_type     = dd::stack<value_type>;
  using successor_stack_type = dd::stack<SDD<C>>;

  const values_type current_values;
  value_stack_type values;
  successor_stack_type successors;

  proto_arc( values_type&& values, value_stack_type&& value_stack
           , successor_stack_type&& successors_stack)
    : current_values(std::move(values)), values(std::move(value_stack))
    , successors(std::move(successors_stack))
  {}
};

template <typename C>
inline
bool
operator==(const proto_arc<C>& lhs, const proto_arc<C>& rhs)
noexcept
{
  return lhs.current_values == rhs.current_values and lhs.values == rhs.values
     and lhs.successors == rhs.successors;
}

/*------------------------------------------------------------------------------------------------*/

/// @brief  A non-terminal node in an SDD.
///
/// For the sake of canonicity, a node shall not exist in several locations, thus we prevent
/// its copy. Also, to enforce this canonicity, we need that nodes have always the same
/// address, thus they can't be moved to an other memory location once created.
template <typename C>
class proto_node final
{
  // Can't copy a proto_node.
  proto_node& operator=(const proto_node&) = delete;
  proto_node(const proto_node&) = delete;

  // Can't move a proto_node.
  proto_node& operator=(proto_node&&) = delete;
  proto_node(proto_node&&) = delete;

public:

  /// @brief The type of the valuation of this node.
  using values_type = typename C::Values;

  using value_type = typename values_type::value_type;

  using arcs_type = std::vector<proto_arc<C>>;

  using const_iterator = typename arcs_type::const_iterator;

private:

  const arcs_type arcs_;

public:

  proto_node(arcs_type&& arcs)
    : arcs_(std::move(arcs))
  {}

  /// @brief Get the beginning of arcs.
  ///
  /// O(1).
  const_iterator
  begin()
  const noexcept
  {
    return arcs_.begin();
  }

  /// @brief Get the end of arcs.
  ///
  /// O(1).
  const_iterator
  end()
  const noexcept
  {
    return arcs_.end();
  }

  const arcs_type&
  arcs()
  const noexcept
  {
    return arcs_;
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @brief Equality of two proto_node.
/// @related proto_node
///
/// O(1) if nodes don't have the same number of arcs; otherwise O(n) where n is the number of
/// arcs.
template <typename C>
inline
bool
operator==(const proto_node<C>& lhs, const proto_node<C>& rhs)
noexcept
{
  return lhs.arcs() == rhs.arcs();
}

/// @brief Export a proto_node to a stream.
/// @related proto_node
template <typename C>
std::ostream&
operator<<(std::ostream& os, const proto_node<C>& n)
{
  os << "proto_node " << &n << ":" << std::endl;
  for (const auto& arc : n)
  {
    os << "  arc " << &n << ":" << std::endl;
    os << "    current values: " << arc.current_values << std::endl;
    os << "    values stack: " << arc.values << std::endl;
    os << "    successors stack: |";
    for (const auto& successor : arc.successors.elements)
    {
      os << &successor << ",";
    }
    os << "|";
  }
  os << std::endl;
  for (const auto& arc : n)
  {
    for (const auto& successor : arc.successors.elements)
    {
      os << successor;
    }
  }
  return os << std::endl;
}

/*------------------------------------------------------------------------------------------------*/

} // namespace sdd

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @brief Hash specialization for sdd::proto_arc
template <typename C>
struct hash<sdd::proto_arc<C>>
{
  std::size_t
  operator()(const sdd::proto_arc<C>& n)
  const noexcept /*(noexcept(???))*/
  {
    std::size_t seed = sdd::util::hash(n.current_values);
    sdd::util::hash_combine(seed, n.values);
    sdd::util::hash_combine(seed, n.successors);
    return seed;
  }
};

/// @brief Hash specialization for sdd::proto_node
template <typename C>
struct hash<sdd::proto_node<C>>
{
  std::size_t
  operator()(const sdd::proto_node<C>& n)
  const noexcept(noexcept(sdd::util::hash(n.begin(), n.end())))
  {
    return sdd::util::hash(n.begin(), n.end());
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std

#endif // _SDD_DD_PROTO_NODE_HH_
<commit_msg>Make proto_arc movable.<commit_after>#ifndef _SDD_DD_PROTO_NODE_HH_
#define _SDD_DD_PROTO_NODE_HH_

#include <algorithm>  // equal, for_each
#include <functional> // hash
#include <iosfwd>
#include <vector>

#include "sdd/dd/definition.hh"
#include "sdd/dd/stack.hh"
#include "sdd/util/hash.hh"

namespace sdd {

/*------------------------------------------------------------------------------------------------*/

template <typename C>
struct proto_arc
{
  using values_type = typename C::Values;
  using value_type = typename values_type::value_type;

  using value_stack_type     = dd::stack<value_type>;
  using successor_stack_type = dd::stack<SDD<C>>;

  values_type current_values;
  value_stack_type values;
  successor_stack_type successors;

  proto_arc(proto_arc&&) = default;
  proto_arc& operator=(proto_arc&&) = default;

  proto_arc( values_type&& values, value_stack_type&& value_stack
           , successor_stack_type&& successors_stack)
    : current_values(std::move(values)), values(std::move(value_stack))
    , successors(std::move(successors_stack))
  {}
};

template <typename C>
inline
bool
operator==(const proto_arc<C>& lhs, const proto_arc<C>& rhs)
noexcept
{
  return lhs.current_values == rhs.current_values and lhs.values == rhs.values
     and lhs.successors == rhs.successors;
}

/*------------------------------------------------------------------------------------------------*/

/// @brief  A non-terminal node in an SDD.
///
/// For the sake of canonicity, a node shall not exist in several locations, thus we prevent
/// its copy. Also, to enforce this canonicity, we need that nodes have always the same
/// address, thus they can't be moved to an other memory location once created.
template <typename C>
class proto_node final
{
  // Can't copy a proto_node.
  proto_node& operator=(const proto_node&) = delete;
  proto_node(const proto_node&) = delete;

  // Can't move a proto_node.
  proto_node& operator=(proto_node&&) = delete;
  proto_node(proto_node&&) = delete;

public:

  /// @brief The type of the valuation of this node.
  using values_type = typename C::Values;

  using value_type = typename values_type::value_type;

  using arcs_type = std::vector<proto_arc<C>>;

  using const_iterator = typename arcs_type::const_iterator;

private:

  const arcs_type arcs_;

public:

  proto_node(arcs_type&& arcs)
    : arcs_(std::move(arcs))
  {}

  /// @brief Get the beginning of arcs.
  ///
  /// O(1).
  const_iterator
  begin()
  const noexcept
  {
    return arcs_.begin();
  }

  /// @brief Get the end of arcs.
  ///
  /// O(1).
  const_iterator
  end()
  const noexcept
  {
    return arcs_.end();
  }

  const arcs_type&
  arcs()
  const noexcept
  {
    return arcs_;
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @brief Equality of two proto_node.
/// @related proto_node
///
/// O(1) if nodes don't have the same number of arcs; otherwise O(n) where n is the number of
/// arcs.
template <typename C>
inline
bool
operator==(const proto_node<C>& lhs, const proto_node<C>& rhs)
noexcept
{
  return lhs.arcs() == rhs.arcs();
}

/// @brief Export a proto_node to a stream.
/// @related proto_node
template <typename C>
std::ostream&
operator<<(std::ostream& os, const proto_node<C>& n)
{
  os << "proto_node " << &n << ":" << std::endl;
  for (const auto& arc : n)
  {
    os << "  arc " << &n << ":" << std::endl;
    os << "    current values: " << arc.current_values << std::endl;
    os << "    values stack: " << arc.values << std::endl;
    os << "    successors stack: |";
    for (const auto& successor : arc.successors.elements)
    {
      os << &successor << ",";
    }
    os << "|";
  }
  os << std::endl;
  for (const auto& arc : n)
  {
    for (const auto& successor : arc.successors.elements)
    {
      os << successor;
    }
  }
  return os << std::endl;
}

/*------------------------------------------------------------------------------------------------*/

} // namespace sdd

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @brief Hash specialization for sdd::proto_arc
template <typename C>
struct hash<sdd::proto_arc<C>>
{
  std::size_t
  operator()(const sdd::proto_arc<C>& n)
  const noexcept /*(noexcept(???))*/
  {
    std::size_t seed = sdd::util::hash(n.current_values);
    sdd::util::hash_combine(seed, n.values);
    sdd::util::hash_combine(seed, n.successors);
    return seed;
  }
};

/// @brief Hash specialization for sdd::proto_node
template <typename C>
struct hash<sdd::proto_node<C>>
{
  std::size_t
  operator()(const sdd::proto_node<C>& n)
  const noexcept(noexcept(sdd::util::hash(n.begin(), n.end())))
  {
    return sdd::util::hash(n.begin(), n.end());
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std

#endif // _SDD_DD_PROTO_NODE_HH_
<|endoftext|>


<commit_before>// This file is part of Tasks.
//
// Tasks 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 3 of the License, or
// (at your option) any later version.
//
// Tasks 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 Tasks.  If not, see <http://www.gnu.org/licenses/>.

// associated header
#include "QPConstr.h"

// includes
// RBDyn
#include <MultiBody.h>
#include <MultiBodyConfig.h>

// SCD
#include <SCD/CD/CD_Pair.h>
#include <SCD/S_Object/S_Object.h>

namespace tasks
{

namespace qp
{

MotionConstr::MotionConstr(const rbd::MultiBody& mb):
	fd_(mb),
	cont_(),
	fullJac_(6, mb.nrDof()),
	AEq_(),
	BEq_(),
	XL_(),
	XU_(),
	nrDof_(0),
	nrFor_(0),
	nrTor_(0)
{
}

void MotionConstr::updateNrVars(const rbd::MultiBody& mb,
	int alphaD, int lambda, int torque, const std::vector<Contact>& cont)
{
	cont_.resize(cont.size());

	nrDof_ = alphaD;
	nrFor_ = lambda;
	nrTor_ = torque;

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		cont_[i].jac = rbd::Jacobian(mb, cont[i].bodyId);
		cont_[i].transJac.resize(6, cont_[i].jac.dof());
		cont_[i].points = cont[i].points;
		cont_[i].normals = cont[i].normals;
	}

	AEq_.resize(nrDof_, nrDof_ + nrFor_ + nrTor_);
	BEq_.resize(nrDof_);

	AEq_.setZero();
	BEq_.setZero();

	XL_.resize(nrFor_);
	XU_.resize(nrFor_);

	XL_.fill(0.);
	XU_.fill(std::numeric_limits<double>::infinity());
}

void MotionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	fd_.computeH(mb, mbc);
	fd_.computeC(mb, mbc);

	// H*alphaD - tau - tau_c = -C

	// AEq
	//         nrDof      nrFor            nrTor
	// nrDof [   H      -Sum J_i^t*ni     [0 ... -1]

	AEq_.block(0, 0, nrDof_, nrDof_) = fd_.H();

	int contPos = nrDof_;
	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		const MatrixXd& jac = cont_[i].jac.jacobian(mb, mbc);

		for(std::size_t j = 0; j < cont_[i].points.size(); ++j)
		{
			cont_[i].jac.translateJacobian(jac, mbc, cont_[i].points[j], cont_[i].transJac);
			cont_[i].jac.fullJacobian(mb, cont_[i].transJac, fullJac_);

			AEq_.block(0, contPos, nrDof_, 1) =
				-fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*cont_[i].normals[j];

			contPos += 1;
		}
	}

	AEq_.block(mb.joint(0).dof(), contPos, nrTor_, nrTor_) =
		-MatrixXd::Identity(nrTor_, nrTor_);

	// BEq = -C
	BEq_ = -fd_.C();
}

int MotionConstr::nrEqLine()
{
	return nrDof_;
}

const Eigen::MatrixXd& MotionConstr::AEq() const
{
	return AEq_;
}

const Eigen::VectorXd& MotionConstr::BEq() const
{
	return BEq_;
}

int MotionConstr::beginVar()
{
	return nrDof_;
}

const Eigen::VectorXd& MotionConstr::Lower() const
{
	return XL_;
}

const Eigen::VectorXd& MotionConstr::Upper() const
{
	return XU_;
}

ContactAccConstr::ContactAccConstr(const rbd::MultiBody& mb):
	cont_(),
	fullJac_(6, mb.nrDof()),
	alphaVec_(mb.nrDof()),
	AEq_(),
	BEq_(),
	nrDof_(0),
	nrFor_(0),
	nrTor_(0)
{}

void ContactAccConstr::updateNrVars(const rbd::MultiBody& mb,
	int alphaD, int lambda, int torque, const std::vector<Contact>& cont)
{
	cont_.resize(cont.size());

	nrDof_ = alphaD;
	nrFor_ = lambda;
	nrTor_ = torque;

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		cont_[i].jac = rbd::Jacobian(mb, cont[i].bodyId);
	}

	AEq_.resize(cont_.size()*6 , nrDof_ + nrFor_ + nrTor_);
	BEq_.resize(cont_.size()*6);

	AEq_.setZero();
	BEq_.setZero();
}

void ContactAccConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	rbd::paramToVector(mbc.alpha, alphaVec_);

	// J_i*alphaD + JD_i*alpha = 0

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		// AEq = J_i
		const MatrixXd& jac = cont_[i].jac.jacobian(mb, mbc);
		cont_[i].jac.fullJacobian(mb, jac, fullJac_);
		AEq_.block(i*6, 0, 6, mb.nrDof()) = fullJac_;

		// BEq = -JD_i*alpha
		const MatrixXd& jacDot = cont_[i].jac.jacobianDot(mb, mbc);
		cont_[i].jac.fullJacobian(mb, jacDot, fullJac_);
		BEq_.segment(i*6, 6) = -fullJac_*alphaVec_;
	}
}

int ContactAccConstr::nrEqLine()
{
	return AEq_.rows();
}

const Eigen::MatrixXd& ContactAccConstr::AEq() const
{
	return AEq_;
}

const Eigen::VectorXd& ContactAccConstr::BEq() const
{
	return BEq_;
}

/**
	*													SelfCollisionConstr
	*/

SCD::Matrix4x4 fromSCD(const sva::PTransform& t)
{
	SCD::Matrix4x4 m;
	const Eigen::Matrix3d& rot = t.rotation();
	const Eigen::Vector3d& tran = t.translation();

	for(int i = 0; i < 3; ++i)
	{
		for(int j = 0; j < 3; ++j)
		{
			m(i,j) = rot(j,i);
		}
	}

	m(0,3) = tran(0);
	m(1,3) = tran(1);
	m(2,3) = tran(2);

	return m;
}

SelfCollisionConstr::CollData::CollData(const rbd::MultiBody& mb,
	int body1Id, SCD::S_Object* body1,
	int body2Id, SCD::S_Object* body2,
	double di, double ds, double damping):
		pair(new SCD::CD_Pair(body1, body2)),
		normVecDist(Eigen::Vector3d::Zero()),
		jacB1(rbd::Jacobian(mb, body1Id)),
		jacB2(rbd::Jacobian(mb, body2Id)),
		di(di),
		ds(ds),
		damping(damping),
		body1Id(body1Id),
		body2Id(body2Id),
		body1(mb.bodyIndexById(body1Id)),
		body2(mb.bodyIndexById(body2Id))
{
}

SelfCollisionConstr::CollData::~CollData()
{
  delete pair;
}

SelfCollisionConstr::SelfCollisionConstr(const rbd::MultiBody& mb, double step):
  dataVec_(),
  step_(step),
  nrVars_(0),
  AInEq_(),
  BInEq_(),
  fullJac_(6, mb.nrDof()),
  fullJacDot_(6, mb.nrDof()),
  alphaVec_(mb.nrDof()),
  calcVec_(mb.nrDof())
{
}

void SelfCollisionConstr::addCollision(const rbd::MultiBody& mb,
																				int body1Id, SCD::S_Object* body1,
																				int body2Id, SCD::S_Object* body2,
																				double di, double ds, double damping)
{
	dataVec_.emplace_back(mb, body1Id, body1, body2Id, body2, di, ds, damping);
}

void SelfCollisionConstr::rmCollision(int body1Id, int body2Id)
{
	auto it = std::find_if(dataVec_.begin(), dataVec_.end(),
		[body1Id, body2Id](const CollData& data)
		{
			return data.body1Id == body1Id && data.body2Id == body2Id;
		});

	if(it != dataVec_.end())
	{
		dataVec_.erase(it);
	}
}

void SelfCollisionConstr::reset()
{
	dataVec_.clear();
}

void SelfCollisionConstr::updateNrVars(const rbd::MultiBody& /* mb */,
	int alphaD, int lambda, int torque, const std::vector<Contact>& /* cont */)
{
	nrVars_ = alphaD + lambda + torque;
}

void SelfCollisionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	if(static_cast<unsigned int>(AInEq_.rows()) != dataVec_.size()
		 || AInEq_.cols() != nrVars_)
	{
		AInEq_.resize(dataVec_.size(), nrVars_);
		BInEq_.resize(dataVec_.size());
		AInEq_.setZero();
		BInEq_.setZero();
	}

	rbd::paramToVector(mbc.alpha, alphaVec_);

	int i = 0;
	for(CollData& d: dataVec_)
	{
		SCD::Point3 pb1Tmp, pb2Tmp;

		(*d.pair)[0]->setTransformation(fromSCD(mbc.bodyPosW[d.body1]));
		(*d.pair)[1]->setTransformation(fromSCD(mbc.bodyPosW[d.body2]));

		double dist = d.pair->getClosestPoints(pb1Tmp, pb2Tmp);
		dist = dist >= 0 ? std::sqrt(dist) : -std::sqrt(-dist);

		Vector3d pb1(pb1Tmp[0], pb1Tmp[1], pb1Tmp[2]);
		Vector3d pb2(pb2Tmp[0], pb2Tmp[1], pb2Tmp[2]);

		Eigen::Vector3d normVecDist = (pb1 - pb2)/dist;

		pb1 = (mbc.bodyPosW[d.body1].inv()*sva::PTransform(pb1)).translation();
		pb2 = (mbc.bodyPosW[d.body2].inv()*sva::PTransform(pb2)).translation();

		if(dist < d.di)
		{
			double dampers = d.damping*((dist - d.ds)/(d.di - d.ds));

			Vector3d nf = normVecDist;
			Vector3d onf = d.normVecDist;
			Vector3d dnf = (nf - onf)/step_;

			// Compute body1
			d.jacB1.point(pb1);
			const MatrixXd& jac1 = d.jacB1.jacobian(mb, mbc);
			const MatrixXd& jacDot1 = d.jacB1.jacobianDot(mb, mbc);

			d.jacB1.fullJacobian(mb, jac1, fullJac_);
			d.jacB1.fullJacobian(mb, jacDot1, fullJacDot_);

			double jqdn = ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf)(0);
			double jqdnd = ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*dnf*step_)(0);
			double jdqdn = ((fullJacDot_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf*step_)(0);

			calcVec_ = -fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*nf*step_;

			// Compute body2
			d.jacB2.point(pb2);
			const MatrixXd& jac2 = d.jacB2.jacobian(mb, mbc);
			const MatrixXd& jacDot2 = d.jacB2.jacobianDot(mb, mbc);

			d.jacB2.fullJacobian(mb, jac2, fullJac_);
			d.jacB2.fullJacobian(mb, jacDot2, fullJacDot_);

			jqdn = -((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf)(0);
			jqdnd = -((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*dnf*step_)(0);
			jdqdn = -((fullJacDot_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf*step_)(0);

			calcVec_ += fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*nf*step_;

			// distdot + distdotdot*dt > -damp*((d - ds)/(di - ds))
			AInEq_.block(i, 0, 1, mb.nrDof()) = calcVec_.transpose();
			BInEq_(i) = dampers + jqdn + jqdnd + jdqdn;
		}
		else
		{
			AInEq_.block(i, 0, 1, mb.nrDof()).setZero();
			BInEq_(i) = 0.;
		}

		d.normVecDist = normVecDist;
		++i;
	}
}

int SelfCollisionConstr::nrInEqLine()
{
	return dataVec_.size();
}

const Eigen::MatrixXd& SelfCollisionConstr::AInEq() const
{
	return AInEq_;
}

const Eigen::VectorXd& SelfCollisionConstr::BInEq() const
{
	return BInEq_;
}

} // namespace qp

} // namespace tasks
<commit_msg>Fix auto collision computation.<commit_after>// This file is part of Tasks.
//
// Tasks 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 3 of the License, or
// (at your option) any later version.
//
// Tasks 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 Tasks.  If not, see <http://www.gnu.org/licenses/>.

// associated header
#include "QPConstr.h"

// includes
// RBDyn
#include <MultiBody.h>
#include <MultiBodyConfig.h>

// SCD
#include <SCD/CD/CD_Pair.h>
#include <SCD/S_Object/S_Object.h>

namespace tasks
{

namespace qp
{

MotionConstr::MotionConstr(const rbd::MultiBody& mb):
	fd_(mb),
	cont_(),
	fullJac_(6, mb.nrDof()),
	AEq_(),
	BEq_(),
	XL_(),
	XU_(),
	nrDof_(0),
	nrFor_(0),
	nrTor_(0)
{
}

void MotionConstr::updateNrVars(const rbd::MultiBody& mb,
	int alphaD, int lambda, int torque, const std::vector<Contact>& cont)
{
	cont_.resize(cont.size());

	nrDof_ = alphaD;
	nrFor_ = lambda;
	nrTor_ = torque;

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		cont_[i].jac = rbd::Jacobian(mb, cont[i].bodyId);
		cont_[i].transJac.resize(6, cont_[i].jac.dof());
		cont_[i].points = cont[i].points;
		cont_[i].normals = cont[i].normals;
	}

	AEq_.resize(nrDof_, nrDof_ + nrFor_ + nrTor_);
	BEq_.resize(nrDof_);

	AEq_.setZero();
	BEq_.setZero();

	XL_.resize(nrFor_);
	XU_.resize(nrFor_);

	XL_.fill(0.);
	XU_.fill(std::numeric_limits<double>::infinity());
}

void MotionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	fd_.computeH(mb, mbc);
	fd_.computeC(mb, mbc);

	// H*alphaD - tau - tau_c = -C

	// AEq
	//         nrDof      nrFor            nrTor
	// nrDof [   H      -Sum J_i^t*ni     [0 ... -1]

	AEq_.block(0, 0, nrDof_, nrDof_) = fd_.H();

	int contPos = nrDof_;
	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		const MatrixXd& jac = cont_[i].jac.jacobian(mb, mbc);

		for(std::size_t j = 0; j < cont_[i].points.size(); ++j)
		{
			cont_[i].jac.translateJacobian(jac, mbc, cont_[i].points[j], cont_[i].transJac);
			cont_[i].jac.fullJacobian(mb, cont_[i].transJac, fullJac_);

			AEq_.block(0, contPos, nrDof_, 1) =
				-fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*cont_[i].normals[j];

			contPos += 1;
		}
	}

	AEq_.block(mb.joint(0).dof(), contPos, nrTor_, nrTor_) =
		-MatrixXd::Identity(nrTor_, nrTor_);

	// BEq = -C
	BEq_ = -fd_.C();
}

int MotionConstr::nrEqLine()
{
	return nrDof_;
}

const Eigen::MatrixXd& MotionConstr::AEq() const
{
	return AEq_;
}

const Eigen::VectorXd& MotionConstr::BEq() const
{
	return BEq_;
}

int MotionConstr::beginVar()
{
	return nrDof_;
}

const Eigen::VectorXd& MotionConstr::Lower() const
{
	return XL_;
}

const Eigen::VectorXd& MotionConstr::Upper() const
{
	return XU_;
}

ContactAccConstr::ContactAccConstr(const rbd::MultiBody& mb):
	cont_(),
	fullJac_(6, mb.nrDof()),
	alphaVec_(mb.nrDof()),
	AEq_(),
	BEq_(),
	nrDof_(0),
	nrFor_(0),
	nrTor_(0)
{}

void ContactAccConstr::updateNrVars(const rbd::MultiBody& mb,
	int alphaD, int lambda, int torque, const std::vector<Contact>& cont)
{
	cont_.resize(cont.size());

	nrDof_ = alphaD;
	nrFor_ = lambda;
	nrTor_ = torque;

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		cont_[i].jac = rbd::Jacobian(mb, cont[i].bodyId);
	}

	AEq_.resize(cont_.size()*6 , nrDof_ + nrFor_ + nrTor_);
	BEq_.resize(cont_.size()*6);

	AEq_.setZero();
	BEq_.setZero();
}

void ContactAccConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	rbd::paramToVector(mbc.alpha, alphaVec_);

	// J_i*alphaD + JD_i*alpha = 0

	for(std::size_t i = 0; i < cont_.size(); ++i)
	{
		// AEq = J_i
		const MatrixXd& jac = cont_[i].jac.jacobian(mb, mbc);
		cont_[i].jac.fullJacobian(mb, jac, fullJac_);
		AEq_.block(i*6, 0, 6, mb.nrDof()) = fullJac_;

		// BEq = -JD_i*alpha
		const MatrixXd& jacDot = cont_[i].jac.jacobianDot(mb, mbc);
		cont_[i].jac.fullJacobian(mb, jacDot, fullJac_);
		BEq_.segment(i*6, 6) = -fullJac_*alphaVec_;
	}
}

int ContactAccConstr::nrEqLine()
{
	return AEq_.rows();
}

const Eigen::MatrixXd& ContactAccConstr::AEq() const
{
	return AEq_;
}

const Eigen::VectorXd& ContactAccConstr::BEq() const
{
	return BEq_;
}

/**
	*													SelfCollisionConstr
	*/

SCD::Matrix4x4 fromSCD(const sva::PTransform& t)
{
	SCD::Matrix4x4 m;
	const Eigen::Matrix3d& rot = t.rotation();
	const Eigen::Vector3d& tran = t.translation();

	for(int i = 0; i < 3; ++i)
	{
		for(int j = 0; j < 3; ++j)
		{
			m(i,j) = rot(j,i);
		}
	}

	m(0,3) = tran(0);
	m(1,3) = tran(1);
	m(2,3) = tran(2);

	return m;
}

SelfCollisionConstr::CollData::CollData(const rbd::MultiBody& mb,
	int body1Id, SCD::S_Object* body1,
	int body2Id, SCD::S_Object* body2,
	double di, double ds, double damping):
		pair(new SCD::CD_Pair(body1, body2)),
		normVecDist(Eigen::Vector3d::Zero()),
		jacB1(rbd::Jacobian(mb, body1Id)),
		jacB2(rbd::Jacobian(mb, body2Id)),
		di(di),
		ds(ds),
		damping(damping),
		body1Id(body1Id),
		body2Id(body2Id),
		body1(mb.bodyIndexById(body1Id)),
		body2(mb.bodyIndexById(body2Id))
{
}

SelfCollisionConstr::CollData::~CollData()
{
  delete pair;
}

SelfCollisionConstr::SelfCollisionConstr(const rbd::MultiBody& mb, double step):
  dataVec_(),
  step_(step),
  nrVars_(0),
  AInEq_(),
  BInEq_(),
  fullJac_(6, mb.nrDof()),
  fullJacDot_(6, mb.nrDof()),
  alphaVec_(mb.nrDof()),
  calcVec_(mb.nrDof())
{
}

void SelfCollisionConstr::addCollision(const rbd::MultiBody& mb,
																				int body1Id, SCD::S_Object* body1,
																				int body2Id, SCD::S_Object* body2,
																				double di, double ds, double damping)
{
	dataVec_.emplace_back(mb, body1Id, body1, body2Id, body2, di, ds, damping);
}

void SelfCollisionConstr::rmCollision(int body1Id, int body2Id)
{
	auto it = std::find_if(dataVec_.begin(), dataVec_.end(),
		[body1Id, body2Id](const CollData& data)
		{
			return data.body1Id == body1Id && data.body2Id == body2Id;
		});

	if(it != dataVec_.end())
	{
		dataVec_.erase(it);
	}
}

void SelfCollisionConstr::reset()
{
	dataVec_.clear();
}

void SelfCollisionConstr::updateNrVars(const rbd::MultiBody& /* mb */,
	int alphaD, int lambda, int torque, const std::vector<Contact>& /* cont */)
{
	nrVars_ = alphaD + lambda + torque;
}

void SelfCollisionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
	using namespace Eigen;

	if(static_cast<unsigned int>(AInEq_.rows()) != dataVec_.size()
		 || AInEq_.cols() != nrVars_)
	{
		AInEq_.resize(dataVec_.size(), nrVars_);
		BInEq_.resize(dataVec_.size());
		AInEq_.setZero();
		BInEq_.setZero();
	}

	rbd::paramToVector(mbc.alpha, alphaVec_);

	int i = 0;
	for(CollData& d: dataVec_)
	{
		SCD::Point3 pb1Tmp, pb2Tmp;

		(*d.pair)[0]->setTransformation(fromSCD(mbc.bodyPosW[d.body1]));
		(*d.pair)[1]->setTransformation(fromSCD(mbc.bodyPosW[d.body2]));

		double dist = d.pair->getClosestPoints(pb1Tmp, pb2Tmp);
		dist = dist >= 0 ? std::sqrt(dist) : -std::sqrt(-dist);

		Vector3d pb1(pb1Tmp[0], pb1Tmp[1], pb1Tmp[2]);
		Vector3d pb2(pb2Tmp[0], pb2Tmp[1], pb2Tmp[2]);

		Eigen::Vector3d normVecDist = (pb1 - pb2)/dist;

		pb1 = (mbc.bodyPosW[d.body1].inv()*sva::PTransform(pb1)).translation();
		pb2 = (mbc.bodyPosW[d.body2].inv()*sva::PTransform(pb2)).translation();

		if(dist < d.di)
		{
			double dampers = d.damping*((dist - d.ds)/(d.di - d.ds));

			Vector3d nf = normVecDist;
			Vector3d onf = d.normVecDist;
			Vector3d dnf = (nf - onf)/step_;

			// Compute body1
			d.jacB1.point(pb1);
			const MatrixXd& jac1 = d.jacB1.jacobian(mb, mbc);
			const MatrixXd& jacDot1 = d.jacB1.jacobianDot(mb, mbc);

			d.jacB1.fullJacobian(mb, jac1, fullJac_);
			d.jacB1.fullJacobian(mb, jacDot1, fullJacDot_);

			double jqdn = ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf)(0);
			double jqdnd = ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*dnf*step_)(0);
			double jdqdn = ((fullJacDot_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf*step_)(0);

			calcVec_ = -fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*nf*step_;

			// Compute body2
			d.jacB2.point(pb2);
			const MatrixXd& jac2 = d.jacB2.jacobian(mb, mbc);
			const MatrixXd& jacDot2 = d.jacB2.jacobianDot(mb, mbc);

			d.jacB2.fullJacobian(mb, jac2, fullJac_);
			d.jacB2.fullJacobian(mb, jacDot2, fullJacDot_);

			jqdn -= ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf)(0);
			jqdnd -= ((fullJac_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*dnf*step_)(0);
			jdqdn -= ((fullJacDot_.block(3, 0, 3, fullJac_.cols())*alphaVec_).transpose()*nf*step_)(0);

			calcVec_ += fullJac_.block(3, 0, 3, fullJac_.cols()).transpose()*nf*step_;

			// distdot + distdotdot*dt > -damp*((d - ds)/(di - ds))
			AInEq_.block(i, 0, 1, mb.nrDof()) = calcVec_.transpose();
			BInEq_(i) = dampers + jqdn + jqdnd + jdqdn;
		}
		else
		{
			AInEq_.block(i, 0, 1, mb.nrDof()).setZero();
			BInEq_(i) = 0.;
		}

		d.normVecDist = normVecDist;
		++i;
	}
}

int SelfCollisionConstr::nrInEqLine()
{
	return dataVec_.size();
}

const Eigen::MatrixXd& SelfCollisionConstr::AInEq() const
{
	return AInEq_;
}

const Eigen::VectorXd& SelfCollisionConstr::BInEq() const
{
	return BInEq_;
}

} // namespace qp

} // namespace tasks
<|endoftext|>


<commit_before>
// Resource.cpp
#include "Resource.h"

int Resource::nextID_ = 0;

// Database table for resources
Table *Resource::resource_table = new Table("Resources");
Table *Resource::resource_type_table = new Table("Resource Types");

Resource::Resource() {
  ID_ = nextID_++;
  MLOG(LEV_DEBUG4) << "Resource ID=" << ID_ << ", ptr=" << this << " created.";
}

Resource::~Resource() {
  MLOG(LEV_DEBUG4) << "Resource ID=" << ID_ << ", ptr=" << this << " deleted.";
}

bool Resource::checkEquality(rsrc_ptr other) {
  bool toRet;
  (this->checkQuality(other) && this->checkQuantityEqual(other)) ? toRet = true : toRet = false;
  return toRet;
}

// -------------------------------------------------------------
/*!
  output database related members
*/

// Specific resources
void Resource::define_table(){
  // declare the table columns
  column id("ID","INTEGER");
  column type("Type","INTEGER");
  column amt("Quantity","REAL");
  column creator("OriginatorID","INTEGER");
  // declare the table's primary key
  resource_table->setPrimaryKey(id);
  // add columns to the table
  resource_table->addColumn(id);
  resource_table->addColumn(type);
  resource_table->addColumn(amt);
  resource_table->addColumn(creator);
  // we've now defined the table
  resource_table->tableDefined();
}

void Resource::addToTable(){
  // if we haven't logged the resource type yet, do so
  if ( !this->is_resource_type_logged() ){
    Resource::logNewType();
    this->type_logged();
  }
  // if we haven't yet, define the table
  if ( !resource_table->defined() )
    Resource::define_table();

  // make a row
  // declare data
  data an_id( this->ID() ), a_type( (int)this->type() ),
    an_amt( this->quantity() ), a_creator( this->originatorID() );
  // declare entries
  entry id("ID",an_id), type("Type",a_type), amt("Quantity", an_amt), creator("OriginatorID",a_creator);
  // declare row
  row aRow;
  aRow.push_back(id), aRow.push_back(type), aRow.push_back(amt), aRow.push_back(creator);
  // add the row
  resource_table->addRow(aRow);
  // record this primary key
  pkref_.push_back(id);
}

// Generic Resource Types
void Resource::define_type_table(){
  // declare the table columns
  column type("Type","INTEGER");
  column type_name("Name","VARCHAR(128)");
  column units("Units","VARCHAR(32)");
  // declare the table's primary key
  resource_type_table->setPrimaryKey(type);
  // add columns to the table
  resource_type_table->addColumn(type);
  resource_type_table->addColumn(type_name);
  resource_type_table->addColumn(units);
  // we've now defined the table
  resource_type_table->tableDefined();
}

void Resource::logNewType(){
  // if we haven't yet, define the table
  if ( !resource_type_table->defined() )
    Resource::define_type_table();

  // make a row
  // declare data
  data a_type( (int)this->type() ), a_name( this->type_name() ), a_unit( this->units() );
  // declare entries
  entry type("Type",a_type), name("Name", a_name), units("Units", a_unit);
  // declare row
  row aRow;
  aRow.push_back(type), aRow.push_back(name), aRow.push_back(units);
  // add the row
  resource_type_table->addRow(aRow);
  // record this primary key
  type_pkref_.push_back(type);
}
<commit_msg>added foreign key to resource table<commit_after>
// Resource.cpp
#include "Resource.h"

int Resource::nextID_ = 0;

// Database table for resources
Table *Resource::resource_table = new Table("Resources");
Table *Resource::resource_type_table = new Table("Resource Types");

Resource::Resource() {
  ID_ = nextID_++;
  MLOG(LEV_DEBUG4) << "Resource ID=" << ID_ << ", ptr=" << this << " created.";
}

Resource::~Resource() {
  MLOG(LEV_DEBUG4) << "Resource ID=" << ID_ << ", ptr=" << this << " deleted.";
}

bool Resource::checkEquality(rsrc_ptr other) {
  bool toRet;
  (this->checkQuality(other) && this->checkQuantityEqual(other)) ? toRet = true : toRet = false;
  return toRet;
}

// -------------------------------------------------------------
/*!
  output database related members
*/

// Specific resources
void Resource::define_table(){
  // declare the table columns
  column id("ID","INTEGER");
  column type("Type","INTEGER");
  column amt("Quantity","REAL");
  column creator("OriginatorID","INTEGER");
  // declare the table's primary key
  resource_table->setPrimaryKey(id);
  // add columns to the table
  resource_table->addColumn(id);
  resource_table->addColumn(type);
  resource_table->addColumn(amt);
  resource_table->addColumn(creator);
  // add foreign keys
  foreign_key_ref *fkref;
  foreign_key *fk;
  key myk, theirk;
  //    Resource Types table foreign keys
  theirk.push_back("Type");
  fkref = new foreign_key_ref("Resource Types",theirk);
  //      the resource id
  myk.push_back("Type");
  fk = new foreign_key(myk, (*fkref) );
  resource_table->addForeignKey( (*fk) ); // type references Resource Types' type
  // we've now defined the table
  resource_table->tableDefined();
}

void Resource::addToTable(){
  // if we haven't logged the resource type yet, do so
  if ( !this->is_resource_type_logged() ){
    Resource::logNewType();
    this->type_logged();
  }
  // if we haven't yet, define the table
  if ( !resource_table->defined() )
    Resource::define_table();

  // make a row
  // declare data
  data an_id( this->ID() ), a_type( (int)this->type() ),
    an_amt( this->quantity() ), a_creator( this->originatorID() );
  // declare entries
  entry id("ID",an_id), type("Type",a_type), amt("Quantity", an_amt), creator("OriginatorID",a_creator);
  // declare row
  row aRow;
  aRow.push_back(id), aRow.push_back(type), aRow.push_back(amt), aRow.push_back(creator);
  // add the row
  resource_table->addRow(aRow);
  // record this primary key
  pkref_.push_back(id);
}

// Generic Resource Types
void Resource::define_type_table(){
  // declare the table columns
  column type("Type","INTEGER");
  column type_name("Name","VARCHAR(128)");
  column units("Units","VARCHAR(32)");
  // declare the table's primary key
  resource_type_table->setPrimaryKey(type);
  // add columns to the table
  resource_type_table->addColumn(type);
  resource_type_table->addColumn(type_name);
  resource_type_table->addColumn(units);
  // we've now defined the table
  resource_type_table->tableDefined();
}

void Resource::logNewType(){
  // if we haven't yet, define the table
  if ( !resource_type_table->defined() )
    Resource::define_type_table();

  // make a row
  // declare data
  data a_type( (int)this->type() ), a_name( this->type_name() ), a_unit( this->units() );
  // declare entries
  entry type("Type",a_type), name("Name", a_name), units("Units", a_unit);
  // declare row
  row aRow;
  aRow.push_back(type), aRow.push_back(name), aRow.push_back(units);
  // add the row
  resource_type_table->addRow(aRow);
  // record this primary key
  type_pkref_.push_back(type);
}
<|endoftext|>


<commit_before>/******************************************************************************
    SDLInput.cpp

    Input Events Handling

  Copyright (c) 2013 Jeffrey Carpenter

******************************************************************************/
#include "SDLInput.h"

SDLInput::SDLInput ( void )
{
  #ifdef DEBUG_SDLINPUT_OBJ
    std::cout << "SDLInput::SDLInput (): " << "Hello, world!" << std::endl << std::endl;
  #endif

  if ( SDL_InitSubSystem ( SDL_INIT_JOYSTICK ) == -1 )
  {
    std::cout << "ERR in SDLInput::SDLInput() at: " << SDL_GetError() << std::endl;
    return;
  }

  // Only initialize this joystick if we have yet to
  if ( SDL_JoystickOpened ( 0 ) == 0 )
  {
    SDL_JoystickEventState ( SDL_ENABLE );

    this->joystick = SDL_JoystickOpen ( 0 );

    std::cout << SDL_NumJoysticks() << " joysticks were found.";
    std::cout << std::endl << std::endl;

    if ( SDL_NumJoysticks() > 0 )
    {
      for( int idx = 0; idx < SDL_NumJoysticks(); idx++ )
      {
        std::cout << SDL_JoystickName ( idx ) << std::endl << std::endl;
      }
    }
  }
}

SDLInput::~SDLInput ( void )
{
  #ifdef DEBUG_SDLINPUT_OBJ
    std::cout << "SDLInput::~SDLInput (): " << "Goodbye cruel world!" << std::endl << std::endl;
  #endif

  // Only close joysticks we have opened
  //if ( SDL_JoystickOpened ( 0 ) == 1 )
  //{
    //SDL_JoystickClose ( this->joystick );

    //if ( this->joystick != NULL )
      //this->joystick = NULL;
  //}

  SDL_QuitSubSystem ( SDL_INIT_JOYSTICK );

}

void SDLInput::Input ( void )
{
  while ( SDL_PollEvent ( &input ) ) // not sure how viable / wise this is
  {
    switch ( input.type )
    {
      case SDL_KEYDOWN:
        onKeyDown ( input.key.keysym.sym, input.key.keysym.mod );
      break;

      case SDL_KEYUP:
        onKeyUp ( input.key.keysym.sym, input.key.keysym.mod );
      break;

      case SDL_MOUSEMOTION:
        onMouseMotion ( input.motion.x, input.motion.y );
      break;

      case SDL_MOUSEBUTTONDOWN:
        switch ( input.button.button )
        {
          case SDL_BUTTON_LEFT:
            onMouseLeftButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_MIDDLE:
            onMouseMiddleButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_RIGHT:
            onMouseRightButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_WHEELDOWN: onMouseWheel ( false, true ); break;

          case SDL_BUTTON_WHEELUP: onMouseWheel ( true, false ); break;
        }
      break;

      case SDL_MOUSEBUTTONUP:
        switch ( input.button.button )
        {
          case SDL_BUTTON_LEFT:
            onMouseLeftButtonUp ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_MIDDLE:
            onMouseMiddleButtonUp ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_RIGHT:
            onMouseRightButtonUp ( input.button.x, input.button.y );
          break;
        }
      break;

      case SDL_JOYBUTTONDOWN:
        onJoyButtonDown ( input.jbutton.which, input.jbutton.button );
      break;

      case SDL_JOYBUTTONUP:
        onJoyButtonUp ( input.jbutton.which, input.jbutton.button );
      break;

      case SDL_JOYAXISMOTION:
        onJoyAxis ( input.jaxis.which, input.jaxis.axis, input.jaxis.value );
      break;

      case SDL_QUIT: onExit(); break;

      case SDL_VIDEORESIZE: onResize ( input.resize.w, input.resize.h ); break;

    } // end switch input->type
  }
}

void SDLInput::onExit ( void )
{
  // virtual implementation
}

void SDLInput::onResize ( unsigned int width, unsigned int height )
{
  // virtual implementation
}

void SDLInput::onRestore ( void )
{
  // virtual implementation
}

void SDLInput::onMinimize ( void )
{
  // virtual implementation
}

void SDLInput::onInputFocus ( void )
{
  // virtual implementation
}

void SDLInput::onMouseFocus ( void )
{
  // virtual implementation
}

void SDLInput::onKeyDown ( SDLKey key, SDLMod mod )
{
  // virtual implementation
}

void SDLInput::onKeyUp ( SDLKey key, SDLMod mod )
{
  // virtual implementation
}

void SDLInput::onMouseMotion ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseWheel ( bool up, bool down )
{
  // virtual implementation
}

void SDLInput::onMouseLeftButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseMiddleButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseRightButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseLeftButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseMiddleButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseRightButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onJoyButtonDown ( unsigned int which, unsigned int button )
{
  // virtual implementation
}

void SDLInput::onJoyButtonUp ( unsigned int which, unsigned int button )
{
  // virtual implementation
}

void SDLInput::onJoyAxis ( unsigned int which, unsigned int axis, short int value )
{
  // virtual implementation
}
<commit_msg>Ever so slight clean up of joystick init/cleanup code fragments<commit_after>/******************************************************************************
    SDLInput.cpp

    Input Events Handling

  Copyright (c) 2013 Jeffrey Carpenter

******************************************************************************/
#include "SDLInput.h"

SDLInput::SDLInput ( void )
{
  #ifdef DEBUG_SDLINPUT_OBJ
    std::cout << "SDLInput::SDLInput (): " << "Hello, world!" << std::endl << std::endl;
  #endif

  if ( SDL_InitSubSystem ( SDL_INIT_JOYSTICK ) == -1 )
  {
    std::cout << "ERR in SDLInput::SDLInput() at: " << SDL_GetError() << std::endl;
    return;
  }

  this->joystick = NULL;

  // Only initialize this joystick if we have yet to
  if ( SDL_JoystickOpened ( 0 ) == 0 )
  {
    SDL_JoystickEventState ( SDL_ENABLE );

    this->joystick = SDL_JoystickOpen ( 0 );

    std::cout << SDL_NumJoysticks() << " joysticks were found.";
    std::cout << std::endl << std::endl;

    if ( SDL_NumJoysticks() > 0 )
    {
      for( int idx = 0; idx < SDL_NumJoysticks(); idx++ )
      {
        std::cout << SDL_JoystickName ( idx ) << std::endl << std::endl;
      }
    }
  }
}

SDLInput::~SDLInput ( void )
{
  #ifdef DEBUG_SDLINPUT_OBJ
    std::cout << "SDLInput::~SDLInput (): " << "Goodbye cruel world!" << std::endl << std::endl;
  #endif

  // Only close joysticks we have opened
  //if ( SDL_JoystickOpened ( 0 ) == 1 )
  //{
    //SDL_JoystickClose ( this->joystick );

    if ( this->joystick )
      this->joystick = NULL;
  //}

  SDL_QuitSubSystem ( SDL_INIT_JOYSTICK );
}

void SDLInput::Input ( void )
{
  while ( SDL_PollEvent ( &input ) ) // not sure how viable / wise this is
  {
    switch ( input.type )
    {
      case SDL_KEYDOWN:
        onKeyDown ( input.key.keysym.sym, input.key.keysym.mod );
      break;

      case SDL_KEYUP:
        onKeyUp ( input.key.keysym.sym, input.key.keysym.mod );
      break;

      case SDL_MOUSEMOTION:
        onMouseMotion ( input.motion.x, input.motion.y );
      break;

      case SDL_MOUSEBUTTONDOWN:
        switch ( input.button.button )
        {
          case SDL_BUTTON_LEFT:
            onMouseLeftButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_MIDDLE:
            onMouseMiddleButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_RIGHT:
            onMouseRightButtonDown ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_WHEELDOWN: onMouseWheel ( false, true ); break;

          case SDL_BUTTON_WHEELUP: onMouseWheel ( true, false ); break;
        }
      break;

      case SDL_MOUSEBUTTONUP:
        switch ( input.button.button )
        {
          case SDL_BUTTON_LEFT:
            onMouseLeftButtonUp ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_MIDDLE:
            onMouseMiddleButtonUp ( input.button.x, input.button.y );
          break;

          case SDL_BUTTON_RIGHT:
            onMouseRightButtonUp ( input.button.x, input.button.y );
          break;
        }
      break;

      case SDL_JOYBUTTONDOWN:
        onJoyButtonDown ( input.jbutton.which, input.jbutton.button );
      break;

      case SDL_JOYBUTTONUP:
        onJoyButtonUp ( input.jbutton.which, input.jbutton.button );
      break;

      case SDL_JOYAXISMOTION:
        onJoyAxis ( input.jaxis.which, input.jaxis.axis, input.jaxis.value );
      break;

      case SDL_QUIT: onExit(); break;

      case SDL_VIDEORESIZE: onResize ( input.resize.w, input.resize.h ); break;

    } // end switch input->type
  }
}

void SDLInput::onExit ( void )
{
  // virtual implementation
}

void SDLInput::onResize ( unsigned int width, unsigned int height )
{
  // virtual implementation
}

void SDLInput::onRestore ( void )
{
  // virtual implementation
}

void SDLInput::onMinimize ( void )
{
  // virtual implementation
}

void SDLInput::onInputFocus ( void )
{
  // virtual implementation
}

void SDLInput::onMouseFocus ( void )
{
  // virtual implementation
}

void SDLInput::onKeyDown ( SDLKey key, SDLMod mod )
{
  // virtual implementation
}

void SDLInput::onKeyUp ( SDLKey key, SDLMod mod )
{
  // virtual implementation
}

void SDLInput::onMouseMotion ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseWheel ( bool up, bool down )
{
  // virtual implementation
}

void SDLInput::onMouseLeftButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseMiddleButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseRightButtonDown ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseLeftButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseMiddleButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onMouseRightButtonUp ( unsigned int x, unsigned int y )
{
  // virtual implementation
}

void SDLInput::onJoyButtonDown ( unsigned int which, unsigned int button )
{
  // virtual implementation
}

void SDLInput::onJoyButtonUp ( unsigned int which, unsigned int button )
{
  // virtual implementation
}

void SDLInput::onJoyAxis ( unsigned int which, unsigned int axis, short int value )
{
  // virtual implementation
}
<|endoftext|>