Initial commit

.gitattributes

+# Auto detect text files and perform LF normalization
+* text=auto

.gitignore

+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore
+
+# User-specific files
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+
+# Visual Studio 2015 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUNIT
+*.VisualState.xml
+TestResult.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+**/Properties/launchSettings.json
+
+*_i.c
+*_p.c
+*_i.h
+*.ilk
+*.meta
+*.obj
+*.pch
+*.pdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*.log
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# JustCode is a .NET coding add-in
+.JustCode
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# The packages folder can be ignored because of Package Restore
+**/packages/*
+# except build/, which is used as an MSBuild target.
+!**/packages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/packages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Typescript v1 declaration files
+typings/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# JetBrains Rider
+.idea/
+*.sln.iml
+
+# CodeRush
+.cr/
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+#the lib folder
+lib/
+lib.zip
\ No newline at end of file

README.md

+# python_simbicon
+
+This project was created to make the code of the Generalized biped controller up to date.
+Here is a link to the original project page: http://www.cs.ubc.ca/~van/papers/2010-TOG-gbwc/index.html
+The first commit of this repository contains the project working with the following components:
+msvc14.0
+python3.6 32 bits
+wxpython4.0.1
+pyopengl3.1.2
+swigwin 3.0.12
+freeglut3.0.0
+glew
+
+!!!WARNING!!! This project require having both visual studio 2015 and 2017 (both in the same language, trying with english).
+The reason is that python need visual 2015 to be able to compile wx python you need to use a visual studio 2015 installation (it was not working with the 14.0 tools from the 2017 version). However to be able to use the mixed mode debugging you need to use visual studio 2017 (you need to make sure that each project is using the visual sution 2014 (140) toolset for compilation).
+
+Note: You have to run cmake to configure ODE before compilling the complete solution.
+
+
+
+note as the folder containing python, freeglut and swig is too large to be store inside the github repository it is store at this link
+https://drive.google.com/open?id=1trYOHyTKkhOZ7UzE0SMN7IuCMXZRihsO
+just unzip it next to the cartwheel-3d folder 
+
+just a a note if using your own python/swig is preferable (or if the lib.zip file is not accesible).
+This zip simple contains the lib folder which contains
+lib\
+  Python36\
+  swigwin-3.0.12\
+  *the various lib and dll for freeglut and glew*
+
+
+
+
+be carefull to be able to use the mixed mode debugging, the use of python36_d.exe was needed (or at least I was succesfull by using it :) ). 
+As such wxpython needs to be manualy compiled in debug config. To do it, download the sources (https://pypi.python.org/pypi/wxPython), open a cmd, go to the folder and type the following : (i know that the option debug+release exists, but since this solution worked I did not bother any further: 
+python.exe .\build.py build --release 
+python.exe .\build.py install
+python.exe .\build.py build --debug 
+python.exe .\build.py install
+
+swig needs no install just download and unzip it (http://www.swig.org/download.html the swigwin one).
+
+to get a working pyopengl, I had to use the .whl file given here https://www.lfd.uci.edu/~gohlke/pythonlibs/#pyopengl
+note: I did not install PyOpenGL_accelerate maybe it could be interesting for better performances.

cartwheel-3d/Core/ActionCollectionPolicy.cpp

+#include "ActionCollectionPolicy.h"
+
+ActionCollectionPolicy::ActionCollectionPolicy(CompositeController* con) : Policy(con){
+
+	actionIndex = -1;
+}
+
+ActionCollectionPolicy::~ActionCollectionPolicy(void){
+}
+
+
+/**
+	this method is used to select the primary and secondary controllers to be run based on the user input.
+*/
+void ActionCollectionPolicy::applyAction(){
+	if (actionIndex >= 0){
+		int pIn = actions[actionIndex].pIndex;
+		int sIn = actions[actionIndex].sIndex;
+		if (con->getStance() == RIGHT_STANCE){
+			for (uint i=0;i<conSwapList.size();i++){
+				if (conSwapList[i].aIndex == pIn){
+					pIn = conSwapList[i].bIndex;
+					break;
+				}
+			}
+			for (uint i=0;i<conSwapList.size();i++){
+				if (conSwapList[i].aIndex == sIn){
+					sIn = conSwapList[i].bIndex;
+					break;
+				}
+			}
+		}
+		con->setControllerInput(pIn, sIn, actions[actionIndex].t);
+	}
+}
+
+/**
+	this method is used to select the primary and secondary controllers to be run based on the user input.
+*/
+void ActionCollectionPolicy::applyActionNoSwap(){
+	if (actionIndex >= 0){
+		int pIn = actions[actionIndex].pIndex;
+		int sIn = actions[actionIndex].sIndex;
+		con->setControllerInput(pIn, sIn, actions[actionIndex].t);
+	}
+}
+
+/**
+	this method is used to read the action list from a file. The method returns the number of actions read.
+*/
+int ActionCollectionPolicy::loadActionsFromFile(char* fName){
+	FILE* f = fopen(fName, "r");
+	char inputLine[200];
+	bool conSwapMode = false;
+	while (!feof(f)){
+		if (readValidLine(inputLine, f)){
+			//read off the action
+			int p, s;
+			double t;
+			double w = 0;
+			if (conSwapMode == false){
+				if (sscanf(inputLine, "%d %d %lf %lf", &p, &s, &t, &w) >= 3){
+					actions.push_back(CompositeAction(p, s, t, w));
+					continue;
+				}
+			}
+			if (strncmp(inputLine, "switchOnRightStance", strlen("switchOnRightStance")) == 0){
+				conSwapMode = true;
+				continue;
+			}
+
+			if (conSwapMode == true && sscanf(inputLine, "%d %d", &p, &s) == 2){
+				conSwapList.push_back(ControllerSwap(p, s));
+				continue;
+			}
+		}
+	}
+	return actions.size();
+}
+

cartwheel-3d/Core/ActionCollectionPolicy.h

+#pragma once
+
+#include "policy.h"
+#include <Core/CompositeController.h>
+
+/**
+	We define a composite action by providing the indices to the primary and secondary controllers and
+	an interpolation parameter. In addition, we'll add a weight, which can be application-dependent.
+*/
+class CompositeAction{
+public:
+	int pIndex;
+	int sIndex;
+	double t;
+	double w;
+
+	CompositeAction(int p, int s, double t, double w = 0){
+		this->pIndex = p;
+		this->sIndex = s;
+		this->t = t;
+		this->w = w;
+	}
+};
+
+class ControllerSwap{
+public:
+	int aIndex;
+	int bIndex;
+	ControllerSwap(int a, int b){
+		this->aIndex = a;
+		this->bIndex = b;
+	}
+};
+
+/**
+	This class is used to store a list of actions (triples of the form: primary controller index, secondary controller index, interpValue).
+	This list is loaded from a file. Also loaded from the file are a set of mappings that indicate weather or not the controllers used should
+	be stance dependent. For instance, one of the actions could be a turn to the left when the character is in a left stance, or turn to the
+	right when the character is in a right stance.
+*/
+class ActionCollectionPolicy : public Policy{
+friend class RLSimulationControlProcess;
+	//this is the list of actions
+	DynamicArray<CompositeAction> actions;
+	//and this is the list of controllers that need to be swapped
+	DynamicArray<ControllerSwap> conSwapList;
+	//this is the selected action
+	int actionIndex;
+public:
+	ActionCollectionPolicy(CompositeController* con);
+	~ActionCollectionPolicy(void);
+
+	inline void setActionIndex(int index){
+		this->actionIndex = index;
+	}
+
+	/**
+		this method is used to read the action list from a file. The method returns the number of actions read.
+	*/
+	int loadActionsFromFile(char* fName);
+
+	/**
+		this method is used to select the primary and secondary controllers to be run based on the user input.
+	*/
+	void applyAction();
+
+	/**
+		this method is used to select the primary and secondary controllers to be run based on the user input.
+		Does not swap the action
+	*/
+	void applyActionNoSwap();
+};
+
+

cartwheel-3d/Core/BalanceFeedback.cpp

+#include "BalanceFeedback.h"
+#include <Core/ConUtils.h>
+#include <Physics/Joint.h>
+#include <Core/SimBiController.h>
+
+
+BalanceFeedback::BalanceFeedback(void){
+}
+
+BalanceFeedback::~BalanceFeedback(void){
+}
+
+
+void LinearBalanceFeedback::loadFromFile(FILE* f){
+	if (f == NULL)
+		throwError("File pointer is NULL - cannot read gain coefficients!!");
+
+	//have a temporary buffer used to read the file line by line...
+	char buffer[200];
+
+	//this is where it happens.
+	while (!feof(f)){
+		//get a line from the file...
+		fgets(buffer, 200, f);
+		if (strlen(buffer)>195)
+			throwError("The input file contains a line that is longer than ~200 characters - not allowed");
+		char *line = lTrim(buffer);
+		int lineType = getConLineType(line);
+		switch (lineType) {
+			case CON_FEEDBACK_END:
+				//we're done...
+				return;
+				break;
+			case CON_COMMENT:
+				break;
+			case CON_CV:
+				if (sscanf(line, "%lf", &this->cv)!=1)
+					throwError("A cv value must be specified!");
+				break;
+			case CON_CD:
+				if (sscanf(line, "%lf", &this->cd)!=1)
+					throwError("A cd value must be specified!");
+				break;
+			case CON_D_MIN:
+				if (sscanf(line, "%lf", &this->dMin)!=1)
+					throwError("A dMin value must be specified!");
+				break;
+			case CON_D_MAX:
+				if (sscanf(line, "%lf", &this->dMax)!=1)
+					throwError("A dMax value must be specified!");
+				break;
+			case CON_V_MIN:
+				if (sscanf(line, "%lf", &this->vMin)!=1)
+					throwError("A vMin value must be specified!");
+				break;
+			case CON_V_MAX:
+				if (sscanf(line, "%lf", &this->vMax)!=1)
+					throwError("A vMax value must be specified!");
+				break;
+			case CON_FEEDBACK_PROJECTION_AXIS:
+				if (sscanf(line, "%lf %lf %lf", &this->feedbackProjectionAxis.x, &this->feedbackProjectionAxis.y, &this->feedbackProjectionAxis.z)!=3)
+					throwError("The axis for a trajectory is specified by three parameters!");
+				this->feedbackProjectionAxis.toUnit();
+				break;
+			case CON_NOT_IMPORTANT:
+				tprintf("Ignoring input line: \'%s\'\n", line);
+				break;
+			default:
+				throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
+		}
+	}
+	throwError("Incorrect SIMBICON input file: No \'/jointTrajectory\' found ", buffer);
+}
+
+
+/**
+	This method is used to write the state parameters to a file
+*/
+void LinearBalanceFeedback::writeToFile(FILE* f){
+	if (f == NULL)
+		return;
+
+	fprintf( f, "\t\t\t%s linear\n", getConLineString(CON_FEEDBACK_START) );
+
+	fprintf( f, "\t\t\t\t%s %lf %lf %lf\n", getConLineString(CON_FEEDBACK_PROJECTION_AXIS),
+		feedbackProjectionAxis.x,
+		feedbackProjectionAxis.y,
+		feedbackProjectionAxis.z );
+	fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_CD), cd );
+	fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_CV), cv );
+	if (dMin > -1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_D_MIN), dMin);
+	if (dMax < 1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_D_MAX), dMax);
+	if (vMin > -1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_V_MIN), vMin);
+	if (vMax < 1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_V_MAX), vMax);
+
+
+	fprintf( f, "\t\t\t%s\n", getConLineString(CON_FEEDBACK_END) );
+}
+
+
+DoubleStanceFeedback::DoubleStanceFeedback(){
+	feedbackProjectionAxis = Vector3d();
+	totalMultiplier = 15;
+	vMin = -0.3;
+	vMax = 0.3;
+	dMin = -10;
+	dMax = 10;
+
+	minFeedbackValue = 0;
+	maxFeedbackValue = 0;
+}
+
+/**
+	This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
+	phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
+	the center of mass.
+*/
+double DoubleStanceFeedback::getFeedbackContribution(SimBiController* con, Joint* j, double phi, Vector3d d, Vector3d v){
+	RigidBody* theBody;
+	Vector3d tmpP;
+	double offset = 0;
+
+	if (j != NULL)
+		theBody = j->getChild();
+	else
+		theBody = con->character->getRoot();
+
+	//we might want to initialize it (or add to it) some default value in case we are trying to control its projection
+	offset = 0; 
+	double dToUse = theBody->getLocalCoordinates(con->doubleStanceCOMError).dotProductWith(feedbackProjectionAxis);
+	if (dToUse < dMin) dToUse = dMin;
+	if (dToUse > dMax) dToUse = dMax;
+
+
+	double vToUse = theBody->getLocalCoordinates(con->comVelocity).dotProductWith(feedbackProjectionAxis);
+	
+	if (vToUse < vMin) vToUse = vMin;
+	if (vToUse > vMax) vToUse = vMax;
+
+	double err = (dToUse * cd - vToUse * cv + offset);
+
+	double result = err * totalMultiplier;
+/*
+	if (j)
+		tprintf("%s gets: %lf\n", j->getName(), result);
+	else
+		tprintf("root gets: %lf\n", result);
+*/
+	if (result < minFeedbackValue) result = minFeedbackValue;
+	if (result > maxFeedbackValue) result = maxFeedbackValue;
+
+//	if (j == NULL && cv > 0.4)
+//		logPrint("%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", theBody->getLocalCoordinates(con->COMOffset).dotProductWith(feedbackProjectionAxis), dToUse, theBody->getLocalCoordinates(con->comVelocity).dotProductWith(feedbackProjectionAxis), vToUse, result, forceRatioWeight);
+
+	return result;
+}
+
+
+void DoubleStanceFeedback::writeToFile(FILE* f){
+	if (f == NULL)
+		return;
+
+	fprintf( f, "\t\t\t%s COM_SupportCenterFeedback\n", getConLineString(CON_FEEDBACK_START) );
+
+	fprintf(f, "Not yet implemented...\n");
+
+	fprintf( f, "\t\t\t%s\n", getConLineString(CON_FEEDBACK_END) );
+}
+
+
+void DoubleStanceFeedback::loadFromFile(FILE* f){
+	if (f == NULL)
+		throwError("File pointer is NULL - cannot read gain coefficients!!");
+
+	//have a temporary buffer used to read the file line by line...
+	char buffer[200];
+
+	//this is where it happens.
+	while (!feof(f)){
+		//get a line from the file...
+		fgets(buffer, 200, f);
+		if (strlen(buffer)>195)
+			throwError("The input file contains a line that is longer than ~200 characters - not allowed");
+		char *line = lTrim(buffer);
+		int lineType = getConLineType(line);
+		switch (lineType) {
+			case CON_FEEDBACK_END:
+				//we're done...
+				return;
+				break;
+			case CON_COMMENT:
+				break;
+			case CON_D_MIN:
+				if (sscanf(line, "%lf", &this->dMin)!=1)
+					throwError("A dMin value must be specified!");
+				break;
+			case CON_D_MAX:
+				if (sscanf(line, "%lf", &this->dMax)!=1)
+					throwError("A dMax value must be specified!");
+				break;
+			case CON_V_MIN:
+				if (sscanf(line, "%lf", &this->vMin)!=1)
+					throwError("A vMin value must be specified!");
+				break;
+			case CON_V_MAX:
+				if (sscanf(line, "%lf", &this->vMax)!=1)
+					throwError("A vMax value must be specified!");
+				break;
+			case CON_CV:
+				if (sscanf(line, "%lf", &this->cv)!=1)
+					throwError("A cv value must be specified!");
+				break;
+			case CON_CD:
+				if (sscanf(line, "%lf", &this->cd)!=1)
+					throwError("A cd value must be specified!");
+				break;
+			case CON_MIN_FEEDBACK:
+				if (sscanf(line, "%lf", &this->minFeedbackValue)!=1)
+					throwError("A minFeedbackValue value must be specified!");
+				break;
+			case CON_MAX_FEEDBACK:
+				if (sscanf(line, "%lf", &this->maxFeedbackValue)!=1)
+					throwError("A maxFeedbackValue value must be specified!");
+				break;
+			case CON_FEEDBACK_PROJECTION_AXIS:
+				if (sscanf(line, "%lf %lf %lf", &this->feedbackProjectionAxis.x, &this->feedbackProjectionAxis.y, &this->feedbackProjectionAxis.z)!=3)
+					throwError("The axis for a trajectory is specified by three parameters!");
+				this->feedbackProjectionAxis.toUnit();
+				break;
+			case CON_NOT_IMPORTANT:
+				tprintf("Ignoring input line: \'%s\'\n", line);
+				break;
+			default:
+				throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
+		}
+	}
+	throwError("Incorrect SIMBICON input file: No \'/jointTrajectory\' found ", buffer);
+}
+
+
+

cartwheel-3d/Core/BalanceFeedback.h

+/*
+	Simbicon 1.5 Controller Editor Framework, 
+	Copyright 2009 Stelian Coros, Philippe Beaudoin and Michiel van de Panne.
+	All rights reserved. Web: www.cs.ubc.ca/~van/simbicon_cef
+
+	This file is part of the Simbicon 1.5 Controller Editor Framework.
+
+	Simbicon 1.5 Controller Editor Framework is free software: you can 
+	redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation, either version 3 of the License, or
+	(at your option) any later version.
+
+	Simbicon 1.5 Controller Editor Framework 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 General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with Simbicon 1.5 Controller Editor Framework. 
+	If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+#include <Utils/Observable.h>
+#include <MathLib/Vector3d.h>
+
+
+class SimBiController;
+class Joint;
+
+/**
+	This generic class provides an interface for classes that provide balance feedback for controllers for physically simulated characters.
+*/
+
+class BalanceFeedback : public Observable {
+public:
+	BalanceFeedback(void);
+	virtual ~BalanceFeedback(void);
+	/**
+		This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
+		phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
+		the center of mass.
+	*/
+	virtual double getFeedbackContribution(SimBiController* con, Joint* j, double phi, Vector3d d, Vector3d v) = 0;
+	virtual void writeToFile(FILE* fp) = 0;
+	virtual void loadFromFile(FILE* fp) = 0;
+
+};
+
+/**
+	This class applies feedback that is linear in d and v - i.e. the original simbicon feedback formulation
+*/
+class LinearBalanceFeedback : public BalanceFeedback{
+public:
+	//This vector, dotted with d or v, gives the quantities that should be used in the feedback formula
+	Vector3d feedbackProjectionAxis;
+	//these are the two feedback gains
+	double cd;
+	double cv;
+
+	double vMin, vMax, dMin, dMax;
+public:
+	LinearBalanceFeedback(){
+		feedbackProjectionAxis = Vector3d();
+		cd = cv = 0;
+		vMin = dMin = -1000;
+		vMax = dMax = 1000;
+	}
+
+	virtual ~LinearBalanceFeedback(){
+
+	}
+
+	void setProjectionAxis( const Vector3d& axis ) {
+		feedbackProjectionAxis = axis;
+	}
+	
+	const Vector3d& getProjectionAxis() const {
+		return feedbackProjectionAxis;
+	}
+
+	void setCd( double cd ) {
+		this->cd = cd;
+	}
+
+	double getCd() const { return cd; }
+
+	void setCv( double cv ) {
+		this->cv = cv;
+	}
+
+	double getCv() const { return cv; }
+
+	void setDLimits( double dMin, double dMax ) {
+		this->dMin = dMin;
+		this->dMax = dMax;
+	}
+
+	double getDMin() const { return dMin; }
+	double getDMax() const { return dMax; }
+
+	void setVLimits( double vMin, double vMax ) {
+		this->vMin = vMin;
+		this->vMax = vMax;
+	}
+
+	double getVMin() const { return vMin; }
+	double getVMax() const { return vMax; }
+
+
+
+	/**
+		This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
+		phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
+		the center of mass.
+	*/
+	virtual double getFeedbackContribution(SimBiController* con, Joint* j, double phi, Vector3d d, Vector3d v){
+		double dToUse = d.dotProductWith(feedbackProjectionAxis);
+		double vToUse = v.dotProductWith(feedbackProjectionAxis);
+		if (dToUse < dMin) dToUse = dMin;
+		if (vToUse < vMin) vToUse = vMin;
+		if (dToUse > dMax) dToUse = dMax;
+		if (vToUse > vMax) vToUse = vMax;
+
+		return dToUse * cd + vToUse * cv;
+	}
+
+	virtual void writeToFile(FILE* fp);
+	virtual void loadFromFile(FILE* fp);
+
+
+};
+
+
+class DoubleStanceFeedback : public BalanceFeedback{
+public:
+	//This vector, dotted with d or v, gives the quantities that should be used in the feedback formula
+	Vector3d feedbackProjectionAxis;
+
+	//this is the max value of the feedback
+	double maxFeedbackValue;
+	//and this is the lowest value of the feedback
+	double minFeedbackValue;
+
+	double cd;
+	double cv;
+	double totalMultiplier;
+	double vMin, vMax, dMin, dMax;
+
+	DoubleStanceFeedback();
+
+	virtual ~DoubleStanceFeedback(){
+
+	}
+
+	/**
+		This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the
+		phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of
+		the center of mass.
+	*/
+	virtual double getFeedbackContribution(SimBiController* con, Joint* j, double phi, Vector3d d, Vector3d v);
+	virtual void writeToFile(FILE* fp);
+	virtual void loadFromFile(FILE* fp);
+
+};
+
+

cartwheel-3d/Core/BehaviourController.cpp

+#include "BehaviourController.h"
+#include <MathLib/Trajectory.h>
+
+BehaviourController::BehaviourController(Character* b, IKVMCController* llc, WorldOracle* w){
+	this->bip = b;
+	this->lowLCon = llc;
+	this->wo = w;
+
+	//we should estimate these from the character info...
+	legLength = 1;
+	ankleBaseHeight = 0.04;
+	shouldPreventLegIntersections = true;
+
+	desiredHeading = 0;
+	ubSagittalLean = 0;
+	ubCoronalLean = 0;
+	ubTwist = 0;
+	duckWalk = 0;
+	duckWalk = 0;
+	velDSagittal = 0;
+	velDCoronal = 0;
+	kneeBend = 0;
+	coronalStepWidth = 0.1;
+
+	stepTime = 0.6;
+	stepHeight = 0;
+}
+
+BehaviourController::~BehaviourController(void){
+
+}
+
+/**
+	ask for a heading...
+*/
+void BehaviourController::requestHeading(double v){
+	desiredHeading = v;
+}
+
+void BehaviourController::setElbowAngles(double leftElbowAngle, double rightElbowAngle){
+	double stanceElbowAngle = (lowLCon->stance == LEFT_STANCE)?(leftElbowAngle):(rightElbowAngle);
+	double swingElbowAngle = (lowLCon->stance == LEFT_STANCE)?(rightElbowAngle):(leftElbowAngle);
+
+	SimBiConState* curState = lowLCon->states[lowLCon->FSMStateIndex];
+	Trajectory* tmpTraj = curState->getTrajectory("STANCE_Elbow");
+	if (tmpTraj != NULL)
+		tmpTraj->components[0]->offset = stanceElbowAngle;
+	tmpTraj = curState->getTrajectory("SWING_Elbow");
+	if (tmpTraj != NULL)
+		tmpTraj->components[0]->offset = swingElbowAngle * -1;
+}
+
+void BehaviourController::setShoulderAngles(double leftTwist, double rightTwist, double leftAdduction, double rightAdduction, double leftSwing, double rightSwing){
+	double stanceTwist = (lowLCon->stance == LEFT_STANCE)?(leftTwist):(rightTwist);
+	double stanceAdd = (lowLCon->stance == LEFT_STANCE)?(leftAdduction):(rightAdduction);
+	double stanceSwing = (lowLCon->stance == LEFT_STANCE)?(leftSwing):(rightSwing);
+
+	double swingTwist = (lowLCon->stance == RIGHT_STANCE)?(leftTwist):(rightTwist);
+	double swingAdd = (lowLCon->stance == RIGHT_STANCE)?(leftAdduction):(rightAdduction);
+	double swingSwing = (lowLCon->stance == RIGHT_STANCE)?(leftSwing):(rightSwing);
+
+	SimBiConState* curState = lowLCon->states[lowLCon->FSMStateIndex];
+	Trajectory* tmpTraj = curState->getTrajectory("STANCE_Shoulder");
+	if (tmpTraj != NULL){
+		tmpTraj->components[0]->offset = stanceTwist;
+		tmpTraj->components[1]->offset = stanceAdd;
+		tmpTraj->components[2]->offset = stanceSwing;
+
+	}
+	tmpTraj = curState->getTrajectory("SWING_Shoulder");
+	if (tmpTraj != NULL){
+		tmpTraj->components[0]->offset = swingTwist;
+		tmpTraj->components[1]->offset = swingAdd * -1;
+		tmpTraj->components[2]->offset = swingSwing;
+	}
+}
+
+/**
+	a set of useful virtual functions, whose behavior can be overridden
+*/
+void BehaviourController::setUpperBodyPose(double leanSagittal, double leanCoronal, double twist){
+	int sign = (lowLCon->stance == LEFT_STANCE)?(-1):(1);
+	SimBiConState* curState = lowLCon->states[lowLCon->getFSMState()];
+	Trajectory* tmpTraj = curState->getTrajectory("root");
+	if (tmpTraj != NULL){
+		tmpTraj->components[2]->offset = leanSagittal;
+		tmpTraj->components[1]->offset = leanCoronal * sign;
+		tmpTraj->components[0]->offset = twist * sign * 0;
+	}
+	tmpTraj = curState->getTrajectory("pelvis_lowerback");
+	if (tmpTraj != NULL){
+		tmpTraj->components[2]->offset = leanSagittal*1.5;
+		tmpTraj->components[1]->offset = leanCoronal *1.5* sign;
+		tmpTraj->components[0]->offset = twist * 1.5 * sign;
+	}
+	tmpTraj = curState->getTrajectory("lowerback_torso");
+	if (tmpTraj != NULL){
+		tmpTraj->components[2]->offset = leanSagittal * 2.5;
+		tmpTraj->components[1]->offset = leanCoronal * 2.5* sign;
+		tmpTraj->components[0]->offset = twist * sign;
+	}
+	tmpTraj = curState->getTrajectory("torso_head");
+	if (tmpTraj != NULL){
+		tmpTraj->components[2]->offset = leanSagittal * 3.0;
+		tmpTraj->components[1]->offset = leanCoronal * 1 * sign;
+		tmpTraj->components[0]->offset = twist * 3.0 * sign;
+	}
+}
+
+void BehaviourController::setKneeBend(double v, bool swingAlso){
+	SimBiConState* curState = lowLCon->states[lowLCon->FSMStateIndex];
+	Trajectory* tmpTraj = curState->getTrajectory("root");
+
+	tmpTraj = curState->getTrajectory("STANCE_Knee");
+	tmpTraj->components[0]->offset = v;
+
+	if (swingAlso == true){
+		tmpTraj = curState->getTrajectory("SWING_Knee");
+		tmpTraj->components[0]->offset = v;
+	}
+}
+
+void BehaviourController::setDuckWalkDegree(double v){
+	boundToRange(&v, -2, 2);
+	Vector3d rotAxis = Vector3d(-1,0,0).rotate(v, Vector3d(0,1,0));
+	lowLCon->swingLegPlaneOfRotation = rotAxis;
+}
+
+void BehaviourController::requestVelocities(double velDS, double velDC){
+	velDSagittal = velDS;
+	velDCoronal = velDC;
+}
+
+/**
+	sets a bunch of parameters to some default initial value
+*/
+void BehaviourController::initializeDefaultParameters(){
+	lowLCon->updateDAndV();
+	desiredHeading = 0;
+}
+
+void BehaviourController::setVelocities(double velDS, double velDC){
+	lowLCon->velDSagittal = velDS;
+	lowLCon->velDCoronal = velDC;
+}
+
+void BehaviourController::setDesiredHeading(double v){
+	lowLCon->desiredHeading = v;
+}
+
+void BehaviourController::requestCoronalStepWidth(double corSW) {
+	coronalStepWidth = corSW;
+}
+
+void BehaviourController::adjustStepHeight(){
+	lowLCon->unplannedForHeight = 0;
+	if (wo != NULL)
+		//the trajectory of the foot was generated without taking the environment into account, so check to see if there are any un-planned bumps (at somepoint in the near future)
+		lowLCon->unplannedForHeight = wo->getWorldHeightAt(lowLCon->swingFoot->state.position + lowLCon->swingFoot->state.velocity * 0.1) * 1.5;
+
+	//if the foot is high enough, we shouldn't do much about it... also, if we're close to the start or end of the
+	//walk cycle, we don't need to do anything... the thing below is a quadratic that is 1 at 0.5, 0 at 0 and 1...
+	double panicIntensity = -4 * lowLCon->phi * lowLCon->phi + 4 * lowLCon->phi;
+	panicIntensity *= getPanicLevel();
+	lowLCon->panicHeight = panicIntensity * 0.05;
+}
+
+/**
+	this method gets called at every simulation time step
+*/
+void BehaviourController::simStepPlan(double dt){
+	lowLCon->updateSwingAndStanceReferences();
+	if (lowLCon->phi <= 0.01)
+		swingFootStartPos = lowLCon->swingFoot->getWorldCoordinates(bip->joints[lowLCon->swingAnkleIndex]->cJPos);
+
+	//compute desired swing foot location...
+	setDesiredSwingFootLocation();
+
+	//set some of these settings
+	setUpperBodyPose(ubSagittalLean, ubCoronalLean, ubTwist);
+	setKneeBend(kneeBend);
+	setDuckWalkDegree((lowLCon->stance == LEFT_STANCE)?(-duckWalk):(duckWalk));
+	setDesiredHeading(desiredHeading);
+	setVelocities(velDSagittal, velDCoronal);
+
+	//adjust for panic mode or unplanned terrain...
+	adjustStepHeight();
+
+	//and see if we're really in trouble...
+//	if (shouldAbort()) onAbort();
+}
+
+void BehaviourController::requestStepTime(double stepTime){
+	this->stepTime = stepTime;
+}
+
+void BehaviourController::requestStepHeight(double stepHeight){
+	this->stepHeight = stepHeight;
+}
+
+/**
+	this method gets called every time the controller transitions to a new state
+*/
+void BehaviourController::conTransitionPlan(){
+	lowLCon->updateSwingAndStanceReferences();
+	lowLCon->updateDAndV();
+	lowLCon->states[0]->stateTime = stepTime;
+
+	lowLCon->updateSwingAndStanceReferences();
+	swingFootStartPos = lowLCon->swingFoot->getCMPosition();
+
+	//now prepare the step information for the following step:
+	lowLCon->swingFootHeightTrajectory.clear();
+	lowLCon->swingFootTrajectoryCoronal.clear();
+	lowLCon->swingFootTrajectorySagittal.clear();
+
+	lowLCon->swingFootHeightTrajectory.addKnot(0, ankleBaseHeight);
+	lowLCon->swingFootHeightTrajectory.addKnot(0.5, ankleBaseHeight + 0.01 + 0.1 + 0 + stepHeight);
+	lowLCon->swingFootHeightTrajectory.addKnot(1, ankleBaseHeight + 0.01);
+
+	lowLCon->swingFootTrajectoryCoronal.addKnot(0,0);
+	lowLCon->swingFootTrajectoryCoronal.addKnot(1,0);
+
+	lowLCon->swingFootTrajectorySagittal.addKnot(0,0);
+	lowLCon->swingFootTrajectorySagittal.addKnot(1,0);
+}
+
+/**
+	returns a panic level which is 0 if val is between minG and maxG, 1 if it's
+	smaller than minB or larger than maxB, and linearly interpolated 
+*/
+double getValueInFuzzyRange(double val, double minB, double minG, double maxG, double maxB){
+	if (val <= minB || val >= maxB)
+		return 1;
+	if (val >= minG && val <= maxG)
+		return 0;
+	if (val > minB && val < minG)
+		return (minG - val) / (minG - minB);
+	if (val > maxG && val < maxB)
+		return (val - maxG) / (maxB - maxG);
+	//the input was probably wrong, so return panic...
+	return 1;
+}
+
+
+/**
+	this method determines the degree to which the character should be panicking
+*/
+double BehaviourController::getPanicLevel(){
+	//the estimate of the panic is given, roughly speaking by the difference between the desired and actual velocities
+	double panicEstimate = 0;
+	panicEstimate += getValueInFuzzyRange(lowLCon->v.z, lowLCon->velDSagittal-0.4, lowLCon->velDSagittal-0.3, lowLCon->velDSagittal+0.3, lowLCon->velDSagittal+0.4);
+	panicEstimate += getValueInFuzzyRange(lowLCon->v.x, lowLCon->velDCoronal-0.3, lowLCon->velDCoronal-0.2, lowLCon->velDCoronal+0.2, lowLCon->velDCoronal+0.3);
+//	boundToRange(&panicEstimate, 0, 1);
+	return panicEstimate/2;
+}
+
+/**
+	this method determines if the character should aim to abort the given plan, and do something else instead (like maybe transition to the
+	next state of the FSM early).
+*/
+bool BehaviourController::shouldAbort(){
+	Vector3d step(lowLCon->getStanceFootPos(), lowLCon->getSwingFootPos());
+	step = lowLCon->getCharacterFrame().inverseRotate(step);
+
+	//TODO: MIGHT NEED TO MAKE THESE CONSTANTS A FUNCTION OF THE LEG LENGTH AT SOME POINT!!!!!
+	if ((step.z > 0.6 && lowLCon->v.z > 0.2) || (step.z < -0.55 && lowLCon->v.z < -0.2))
+		return true;
+	
+	if (lowLCon->stance == LEFT_STANCE){
+		if ((step.x < -0.45 && lowLCon->v.x < -0.2) || (step.x > 0.35 && lowLCon->v.x > 0.2))
+			return true;
+	}else{
+		if ((step.x > 0.45 && lowLCon->v.x > 0.2) || (step.x < -0.35 && lowLCon->v.x < -0.2))
+			return true;
+	}
+
+	return false;
+}
+
+/**
+	this method is used to indicate what the behaviour of the character should be, once it decides to abort its plan.
+*/
+void BehaviourController::onAbort(){
+	//force a premature switch to the next controller state
+	if (lowLCon->phi > 0.2){
+		lowLCon->phi = 1;
+		tprintf("PANIC!!!\n");
+	}
+//	Globals::animationRunning = false;
+//	return;
+}
+
+/**
+	modify the coronal location of the step so that the desired step width results.
+*/
+double BehaviourController::adjustCoronalStepLocation(double IPPrediction){
+	//nothing to do if it's the default value...
+	if (coronalStepWidth < 0.01)
+		return IPPrediction;
+
+	double stepWidth = coronalStepWidth / 2;
+	stepWidth = (lowLCon->stance == LEFT_STANCE)?(-stepWidth):(stepWidth);
+
+	//now for the step in the coronal direction - figure out if the character is still doing well - panic = 0 is good, panic = 1 is bad...
+	double panicLevel = 1;
+	if (lowLCon->stance == LEFT_STANCE){
+		panicLevel = getValueInFuzzyRange(lowLCon->d.x, 1.15 * stepWidth, 0.5 * stepWidth, 0.25 * stepWidth, -0.25 * stepWidth);
+		panicLevel += getValueInFuzzyRange(lowLCon->v.x, 2*stepWidth, stepWidth, -stepWidth, -stepWidth*1.5);
+	}
+	else{
+		panicLevel = getValueInFuzzyRange(lowLCon->d.x, -0.25 * stepWidth, 0.25 * stepWidth, 0.5 * stepWidth, 1.15 * stepWidth);
+		panicLevel += getValueInFuzzyRange(lowLCon->v.x, -stepWidth*1.5, -stepWidth, stepWidth, 2*stepWidth);
+	}
+	boundToRange(&panicLevel, 0, 1);
+	Trajectory1d offsetMultiplier;
+	offsetMultiplier.addKnot(0.05, 0); offsetMultiplier.addKnot(0.075, 1/2.0);
+	double offset = stepWidth * offsetMultiplier.evaluate_linear(fabs(stepWidth));
+//	if (IPPrediction * stepWidth < 0) offset = 0;
+	//if it's doing well, use the desired step width...
+	IPPrediction = panicLevel * (IPPrediction + offset) + (1-panicLevel) * stepWidth;
+	lowLCon->comOffsetCoronal = (1-panicLevel) * stepWidth;
+
+//	if (panicLevel >= 1)
+//		tprintf("panic level: %lf; d.x = %lf\n", panicLevel, lowLCon->d.x);
+
+	return IPPrediction;
+}
+
+void BehaviourController::requestUpperBodyPose(double leanS, double leanC, double twist){
+	this->ubSagittalLean = leanS;
+	this->ubCoronalLean = leanC;
+	this->ubTwist = twist;
+}
+
+void BehaviourController::requestKneeBend(double kb){
+	this->kneeBend = kb;
+}
+
+void BehaviourController::requestDuckFootedness(double df){
+	this->duckWalk = df;
+}
+
+/**
+	determines weather a leg crossing is bound to happen or not, given the predicted final desired position	of the swing foot.
+	The suggested via point is expressed in the character frame, relative to the COM position...The via point is only suggested
+	if an intersection is detected.
+*/
+bool BehaviourController::detectPossibleLegCrossing(const Vector3d& swingFootPos, Vector3d* viaPoint){
+	//first, compute the world coords of the swing foot pos, since this is in the char. frame 
+	Point3d desSwingFootPos = lowLCon->characterFrame.rotate(swingFootPos) + lowLCon->comPosition;
+	//now, this is the segment that starts at the current swing foot pos and ends at the final
+	//swing foot position
+
+	Segment swingFootTraj(lowLCon->swingFoot->state.position, desSwingFootPos); swingFootTraj.a.y = 0; swingFootTraj.b.y = 0;
+	
+	//and now compute the segment originating at the stance foot that we don't want the swing foot trajectory to pass...
+	Vector3d segDir = Vector3d(100, 0, 0);
+	if (lowLCon->stance == RIGHT_STANCE) segDir.x = -segDir.x;
+	segDir = lowLCon->stanceFoot->getWorldCoordinates(segDir); segDir.y = 0;
+	Segment stanceFootSafety(lowLCon->stanceFoot->state.position, lowLCon->stanceFoot->state.position + segDir);
+	stanceFootSafety.a.y = 0; stanceFootSafety.b.y = 0;
+
+	//now check to see if the two segments intersect...
+	Segment intersect;
+	stanceFootSafety.getShortestSegmentTo(swingFootTraj, &intersect);
+
+/*
+	predSwingFootPosDebug = desSwingFootPos;predSwingFootPosDebug.y = 0;
+	swingSegmentDebug = swingFootTraj;
+	crossSegmentDebug = stanceFootSafety;
+	viaPointSuggestedDebug.setValues(0,-100,0);
+*/
+
+	//now, if this is too small, then it means the swing leg will cross the stance leg...
+	double safeDist = 0.02;
+	if (Vector3d(intersect.a, intersect.b).length() < safeDist){
+		if (viaPoint != NULL){
+			*viaPoint = lowLCon->stanceFoot->state.position + segDir.unit() * -0.05;
+			(*viaPoint) -= Vector3d(lowLCon->comPosition);
+			*viaPoint = lowLCon->characterFrame.inverseRotate(*viaPoint);
+			viaPoint->y = 0;
+/*
+			viaPointSuggestedDebug = lowLCon->characterFrame.rotate(*viaPoint) + lowLCon->comPosition;
+			viaPointSuggestedDebug.y = 0;
+*/
+		}
+		return true;
+	}
+	
+	return false;
+}
+
+/**
+	determine the estimate desired location of the swing foot, given the etimated position of the COM, and the phase
+*/
+Vector3d BehaviourController::computeSwingFootLocationEstimate(const Point3d& comPos, double phase){
+	Vector3d step = lowLCon->computeIPStepLocation();
+
+	//applying the IP prediction would make the character stop, so take a smaller step if you want it to walk faster, or larger
+	//if you want it to go backwards
+	step.z -= lowLCon->velDSagittal / 20;
+	//and adjust the stepping in the coronal plane in order to account for desired step width...
+	step.x = adjustCoronalStepLocation(step.x);
+
+	boundToRange(&step.z, -0.4 * legLength, 0.4 * legLength);
+	boundToRange(&step.x, -0.4 * legLength, 0.4 * legLength);
+
+	Vector3d result;
+	Vector3d initialStep(comPos, swingFootStartPos);
+	initialStep = lowLCon->characterFrame.inverseRotate(initialStep);
+	//when phi is small, we want to be much closer to where the initial step is - so compute this quantity in character-relative coordinates
+	//now interpolate between this position and initial foot position - but provide two estimates in order to provide some gradient information
+	double t = (1-phase);
+	t = t * t;
+	boundToRange(&t, 0, 1);
+
+	Vector3d suggestedViaPoint;
+	alternateFootTraj.clear();
+	bool needToStepAroundStanceAnkle = false;
+	if (phase < 0.8 && shouldPreventLegIntersections && getPanicLevel() < 0.5)
+		needToStepAroundStanceAnkle = detectPossibleLegCrossing(step, &suggestedViaPoint);
+	if (needToStepAroundStanceAnkle){
+		//use the via point...
+		Vector3d currentSwingStepPos(comPos, lowLCon->swingFoot->state.position);
+		currentSwingStepPos = lowLCon->characterFrame.inverseRotate(initialStep);currentSwingStepPos.y = 0;		
+		//compute the phase for the via point based on: d1/d2 = 1-x / x-phase, where d1 is the length of the vector from
+		//the via point to the final location, and d2 is the length of the vector from the swing foot pos to the via point...
+		double d1 = (step - suggestedViaPoint).length(); double d2 = (suggestedViaPoint - currentSwingStepPos).length(); if (d2 < 0.0001) d2 = d1 + 0.001;
+		double c =  d1/d2;
+		double viaPointPhase = (1+phase*c)/(1+c);
+		//now create the trajectory...
+		alternateFootTraj.addKnot(0, initialStep);
+		alternateFootTraj.addKnot(viaPointPhase, suggestedViaPoint);
+		alternateFootTraj.addKnot(1, step);
+		//and see what the interpolated position is...
+		result = alternateFootTraj.evaluate_catmull_rom(1-t);
+//		tprintf("t: %lf\n", 1-t);
+	}else{
+		result.addScaledVector(step, 1-t);
+		result.addScaledVector(initialStep, t);
+	}
+
+	result.y = 0;
+
+/*
+	suggestedFootPosDebug = result;
+*/
+	return result;
+}
+
+/**
+	determines the desired swing foot location
+*/
+void BehaviourController::setDesiredSwingFootLocation(){
+	Vector3d step = computeSwingFootLocationEstimate(lowLCon->comPosition, lowLCon->phi);
+	lowLCon->swingFootTrajectoryCoronal.setKnotValue(0, step.x);
+	lowLCon->swingFootTrajectorySagittal.setKnotValue(0, step.z);
+
+	double dt = 0.001;
+	step = computeSwingFootLocationEstimate(lowLCon->comPosition + lowLCon->comVelocity * dt, lowLCon->phi+dt);
+	lowLCon->swingFootTrajectoryCoronal.setKnotValue(1, step.x);
+	lowLCon->swingFootTrajectorySagittal.setKnotValue(1, step.z);
+	//to give some gradient information, here's what the position will be a short time later...
+
+	lowLCon->swingFootTrajectorySagittal.setKnotPosition(0, lowLCon->phi);
+	lowLCon->swingFootTrajectorySagittal.setKnotPosition(1, lowLCon->phi+dt);
+
+	lowLCon->swingFootTrajectoryCoronal.setKnotPosition(0, lowLCon->phi);
+	lowLCon->swingFootTrajectoryCoronal.setKnotPosition(1, lowLCon->phi+dt);
+}
+
+
+

cartwheel-3d/Core/BehaviourController.h

+#pragma once
+
+#include <Core/IKVMCController.h>
+#include <MathLib/Segment.h>
+#include <Core/WorldOracle.h>
+
+/**
+	This class implements an intermediate-level controller. Given a low level controller (of the type IKVMCController, for now),
+	this class knows how to set a series of parameters in order to accomplish higher level behaviours.
+
+	NOTE: We will assume a fixed character morphology (i.e. joints & links), and a fixed controller structure 
+	(i.e. trajectories,	components).
+*/
+class BehaviourController{
+protected:
+	Character* bip;
+	IKVMCController* lowLCon;
+	WorldOracle* wo;
+
+	//we need to keep track of the position of the swing foot at the beginning of the step
+	Point3d swingFootStartPos;
+
+	//this is the length of the leg of the character controlled by this controller
+	double legLength;
+	double ankleBaseHeight;
+	bool shouldPreventLegIntersections;
+
+
+	//these are attributes/properties of the motion
+	double desiredHeading;
+	double ubSagittalLean;
+	double ubCoronalLean;
+	double ubTwist;
+	double duckWalk;
+	double velDSagittal;
+	double velDCoronal;
+	double kneeBend;
+	double coronalStepWidth;
+
+	double stepTime;
+	double stepHeight;
+
+public:
+/*
+	//DEBUG ONLY
+	Point3d predSwingFootPosDebug;
+	Point3d viaPointSuggestedDebug;
+	Point3d suggestedFootPosDebug;
+	Segment swingSegmentDebug;
+	Segment crossSegmentDebug;
+*/
+
+	//alternate planned foot trajectory, for cases where we need to go around the stance foot...
+	Trajectory3d alternateFootTraj;
+
+
+	/**
+		a set of useful virtual functions, whose behavior can be overridden
+	*/
+	virtual void setUpperBodyPose(double leanSagittal, double leanCoronal, double twist);
+	virtual void setKneeBend(double v, bool swingAlso = false);
+	virtual void setDuckWalkDegree(double v);
+	virtual void setDesiredHeading(double v);
+	virtual void setVelocities(double velDS, double velDC);
+
+public:
+	BehaviourController(Character* b, IKVMCController* llc, WorldOracle* w = NULL);
+	virtual ~BehaviourController(void);
+
+
+	virtual void adjustStepHeight();
+
+	virtual void setElbowAngles(double leftElbowAngle, double rightElbowAngle);
+	virtual void setShoulderAngles(double leftTwist, double rightTwist, double leftAdduction, double rightAdduction, double leftSwing, double rightSwing);
+
+
+	virtual void requestStepTime(double stepTime);
+	virtual void requestStepHeight(double stepHeight);
+	virtual void requestVelocities(double velDS, double velDC);
+	virtual void requestUpperBodyPose(double leanS, double leanC, double twist);
+	virtual void requestKneeBend(double kb);
+	virtual void requestDuckFootedness(double df);
+	virtual void requestCoronalStepWidth(double corSW);
+
+	double getDesiredStepTime() const { return stepTime; }
+	double getDesiredVelocitySagittal() const { return velDSagittal; }
+	double getCoronalStepWidth() const { return coronalStepWidth; }
+
+	/**
+		determines the desired swing foot location
+	*/
+	virtual void setDesiredSwingFootLocation();
+
+	/**
+		determine the estimate desired location of the swing foot, given the etimated position of the COM, and the phase
+	*/
+	virtual Vector3d computeSwingFootLocationEstimate(const Point3d& comPos, double phase); 
+
+	/**
+		ask for a heading...
+	*/
+	virtual void requestHeading(double v);
+
+	/**
+		sets a bunch of parameters to some default initial value
+	*/
+	virtual void initializeDefaultParameters();
+
+	/**
+		this method gets called at every simulation time step
+	*/
+	virtual void simStepPlan(double dt);
+
+	/**
+		this method gets called every time the controller transitions to a new state
+	*/
+	virtual void conTransitionPlan();
+
+	/**
+		this method determines the degree to which the character should be panicking
+	*/
+	virtual double getPanicLevel();
+
+	/**
+		this method determines if the character should aim to abort the given plan, and do something else instead (like maybe transition to the
+		next state of the FSM early).
+	*/
+	virtual bool shouldAbort();
+
+	/**
+		this method is used to indicate what the behaviour of the character should be, once it decides to abort its plan.
+	*/
+	virtual void onAbort();
+
+	/**
+		determines weather a leg crossing is bound to happen or not, given the predicted final desired position	of the swing foot.
+		The suggested via point is expressed in the character frame, relative to the COM position... The via point is only suggested
+		if an intersection is detected.
+	*/
+	bool detectPossibleLegCrossing(const Vector3d& swingFootPos, Vector3d* viaPoint);
+
+
+	/**
+		modify the coronal location of the step so that the desired step width results.
+	*/
+	double adjustCoronalStepLocation(double IPPrediction);
+};
+
+

cartwheel-3d/Core/BipV3BalanceController.cpp

+#include "BipV3BalanceController.h"
+
+
+BipV3BalanceController::BipV3BalanceController(World* w, Character* b){
+	bip = b;
+	world = w;
+	steppingController = new SimBiController(bip);
+	standingBalanceController = new SimBiController(bip);
+	steppingController->loadFromFile("../data/controllers/bipV3/iWalk.sbc");
+	standingBalanceController->loadFromFile("../data/controllers/bipV3/balance.sbc");
+
+	con = steppingController;
+}
+
+BipV3BalanceController::~BipV3BalanceController(){
+	delete steppingController;
+	delete standingBalanceController;
+}
+
+/**
+	this method is used to set the offsets for any and all components of trajectories..
+*/
+void BipV3BalanceController::applyTrajectoryOffsets(){
+//	return;
+	//huge hack for now...
+
+	SimBiConState* curState = standingBalanceController->states[con->FSMStateIndex];
+
+
+	Trajectory* tmpTraj;
+
+	SimGlobals::rootSagittal = SimGlobals::stanceKnee / 10.0;
+
+	tmpTraj = curState->getTrajectory("root");
+	tmpTraj->components[2]->offset = SimGlobals::rootSagittal;
+	tmpTraj = curState->getTrajectory("pelvis_lowerback");
+	tmpTraj->components[2]->offset = SimGlobals::rootSagittal*1.5;
+	tmpTraj = curState->getTrajectory("lowerback_torso");
+	tmpTraj->components[2]->offset = SimGlobals::rootSagittal*2;
+	tmpTraj = curState->getTrajectory("torso_head");
+	tmpTraj->components[2]->offset = SimGlobals::rootSagittal*2.1;
+
+
+/*
+	tmpTraj = curState->getTrajectory("root");
+	tmpTraj->components[0]->offset = SimGlobals::rootSagittal;
+	tmpTraj = curState->getTrajectory("pelvis_lowerback");
+	tmpTraj->components[0]->offset = SimGlobals::rootSagittal*1.5;
+	tmpTraj = curState->getTrajectory("lowerback_torso");
+	tmpTraj->components[0]->offset = SimGlobals::rootSagittal*2;
+	tmpTraj = curState->getTrajectory("torso_head");
+	tmpTraj->components[0]->offset = SimGlobals::rootSagittal*3;
+*/
+
+/*
+	tmpTraj = curState->getTrajectory("root");
+	tmpTraj->components[1]->offset = SimGlobals::rootSagittal;
+	tmpTraj = curState->getTrajectory("pelvis_lowerback");
+	tmpTraj->components[1]->offset = SimGlobals::rootSagittal*1.5;
+	tmpTraj = curState->getTrajectory("lowerback_torso");
+	tmpTraj->components[1]->offset = SimGlobals::rootSagittal*2;
+	tmpTraj = curState->getTrajectory("torso_head");
+	tmpTraj->components[1]->offset = SimGlobals::rootSagittal*3;
+*/
+
+	tmpTraj = curState->getTrajectory("STANCE_Knee");
+	tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
+	tmpTraj = curState->getTrajectory("SWING_Knee");
+	tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
+
+
+
+/*	
+	tmpTraj = curState->getTrajectory("SWING_Hip");
+	tmpTraj->components[0]->offset = SimGlobals::swingHipSagittal;
+
+//	tmpTraj = curState->getTrajectory("root");
+//	tmpTraj->components[0]->offset = SimGlobals::swingHipSagittal;
+
+//	tmpTraj->components[1]->offset = SimGlobals::swingHipLateral;
+//	tmpTraj = curState->getTrajectory("STANCE_Hip");
+//	tmpTraj->components[0]->offset = 0;
+//	tmpTraj->components[1]->offset = 0;
+
+//	tmpTraj = curState->getTrajectory("STANCE_Ankle");
+//	tmpTraj->components[0]->offset = SimGlobals::stanceAngleSagittal;
+//	tmpTraj->components[1]->offset = SimGlobals::stanceAngleLateral;
+//	tmpTraj = curState->getTrajectory("SWING_Ankle");
+//	tmpTraj->components[0]->offset = 0;
+//	tmpTraj->components[1]->offset = 0;
+
+	tmpTraj = curState->getTrajectory("STANCE_Knee");
+	tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
+	tmpTraj = curState->getTrajectory("SWING_Knee");
+	tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
+*/
+}
+
+
+bool BipV3BalanceController::runASimulationStep(){
+	applyTrajectoryOffsets();
+
+	//compute the vectors used to measure the error from the balancing goal
+	Vector3d comOffsetError = con->doubleStanceCOMError; comOffsetError.y = 0;
+	Vector3d comVelError = con->v; comVelError.y = 0;
+
+	double comError = comOffsetError.length();
+	double comVError = comVelError.length();
+
+	//the character relative position of the left and right feet are here...
+	Vector3d leftFootPos = con->characterFrame.inverseRotate(Vector3d(con->root->state.position,con->lFoot->state.position));
+	Vector3d rightFootPos = con->characterFrame.inverseRotate(Vector3d(con->root->state.position, con->rFoot->state.position));
+
+	//these are the total forces on the stance and swing feet
+	double f1 = /*con->forceStanceHeel.y + con->forceStanceToe.y*/0.5;
+	double f2 = /*con->forceSwingHeel.y + con->forceSwingToe.y*/0.5;
+
+	double tot = f1+f2;
+
+	if (tot < 1){
+		f1 = 0;
+		tot = 1;
+	}
+
+	//and the ratio of vertical forces on the two feet
+	double ratio = f1 / tot;
+
+//	tprintf("(%lf %lf %lf)\n", ratio, comError, comVelError);
+
+
+	//if we're walking, see if we can transition to the balance controller
+	if (con != standingBalanceController){
+		if (ratio >= 0.25 && ratio <= 0.75){
+//			tprintf("can stand!!! (%lf %lf)\n", comError, comVelError);
+			//both feet are on the ground, so make sure that the com position and velocity are not too far off...
+			//compute the character-relative coordinates of the left and right foot
+			tprintf("left: %lf, right: %lf, (%lf %lf)\n", leftFootPos.x, rightFootPos.x, comError, comVelError);
+			double maxComError = 0, maxComVError = 0;
+			if (comOffsetError.unit().dotProductWith(comVelError.unit())<-0.5){
+				maxComError = 0.05;
+				maxComVError = 0.4;
+			}else{
+				maxComError = 0.03;
+				maxComVError = 0.2;
+			}
+			if (comError < maxComError && comVError < maxComVError && leftFootPos.x > 0.03 && leftFootPos.x < 0.1 && -rightFootPos.x > 0.03 && -rightFootPos.x < 0.1){
+				con = standingBalanceController;
+				tprintf("can stand!!! (%lf %lf)\n", comError, comVelError);
+			}
+		}
+	}else{
+		//see if we need to start walking...
+		if (comError > 0.05 || comVError > 0.35 /*|| (!(con->stanceHeelInContact || con->stanceToeInContact) && (con->swingHeelInContact || con->swingToeInContact))*/){
+			con = steppingController;
+			con->phi = 0;
+			if (comVelError.x < 0)
+				con->setStance(LEFT_STANCE);
+			else
+				con->setStance(RIGHT_STANCE);
+			tprintf("can't stand!!! (%lf %lf)\n", comError, comVelError);
+		}
+	}
+
+
+
+	con->computeTorques(world->getContactForces());
+
+//	tprintf("%lf\n", con->comPosition.y);
+
+	con->applyTorques();
+	world->advanceInTime(SimGlobals::dt);
+	return (con->advanceInTime(SimGlobals::dt, world->getContactForces()) != -1);
+}
+
+
+

cartwheel-3d/Core/BipV3BalanceController.h

+#pragma once
+
+#include <Core/Character.h>
+#include <Core/SimBiController.h>
+
+/**
+	This class uses a balance controller that is switched to a walk controller when large enough disturbances are intorduced.
+*/
+class BipV3BalanceController{
+private:
+	//this is the active controller that is used for the character
+	SimBiController* con;
+	//these are the controllers used for balance and stepping
+	SimBiController* steppingController;
+	SimBiController* standingBalanceController;
+	//and this is the controller that will be controlled
+	Character* bip;
+	World* world;
+
+public:
+	BipV3BalanceController(World* w, Character* b);
+	~BipV3BalanceController();
+
+	/**
+		this method is used to set the offsets for any and all components of trajectories..
+	*/
+	void applyTrajectoryOffsets();
+
+	bool runASimulationStep();
+
+	SimBiController* getActiveController(){return con;}
+
+};
+

cartwheel-3d/Core/Character.cpp

+#include "Character.h"
+#include <Utils/Utils.h>
+#include <stdio.h>
+
+/**
+	the constructor
+*/
+Character::Character() : ArticulatedFigure() {
+}
+
+/**
+	the destructor
+*/
+Character::~Character(void){
+	//nothing to do. We'll let whoever created the world deal with freeing it up
+}
+
+/**
+	This method is used to populate the relative orientation of the parent and child bodies of joint i.
+*/
+void Character::getRelativeOrientation(int i, Quaternion* qRel){
+	//rotation from child frame to world, and then from world to parent == rotation from child to parent
+	joints[i]->computeRelativeOrientation(*qRel);
+}
+
+/**
+	This method is used to get the relative angular velocities of the parent and child bodies of joint i,
+	expressed in parent's local coordinates. 
+	We'll assume that i is in the range 0 - joints.size()-1!!!
+*/
+void Character::getRelativeAngularVelocity(int i, Vector3d* wRel){
+	*wRel = joints[i]->child->state.angularVelocity - joints[i]->parent->state.angularVelocity;
+	//we will store wRel in the parent's coordinates, to get an orientation invariant expression for it
+	*wRel = joints[i]->parent->getLocalCoordinates(*wRel);
+}
+
+
+/**
+	This method populates the dynamic array passed in with the state of the character.
+	For the state, we consider the 13-dimensional state of the root, and then only
+	the relative orientation and angular velocity (as measured from the parent) for
+	every other link. The velocities of the CM are derived from this information,
+	using the velocity propagation technique (if that's what it is called).		
+	The only thing we will not be storing explicitly is the positions of the CMs of the rigid bodies. 
+	The order in which the bodies appear is given by the array of joints. 
+	This works under the assumption that in the joint 
+	sequence, the parent of any rigid body appears before its children (assuming that for each joint
+	we read the parent first and then the child). 
+*/
+void Character::getState(ReducedCharacterStateArray* state){
+	updateJointOrdering();
+	//we'll push the root's state information - ugly code....
+	state->push_back(root->state.position.x);
+	state->push_back(root->state.position.y);
+	state->push_back(root->state.position.z);
+
+	state->push_back(root->state.orientation.s);
+	state->push_back(root->state.orientation.v.x);
+	state->push_back(root->state.orientation.v.y);
+	state->push_back(root->state.orientation.v.z);
+
+	state->push_back(root->state.velocity.x);
+	state->push_back(root->state.velocity.y);
+	state->push_back(root->state.velocity.z);
+
+	state->push_back(root->state.angularVelocity.x);
+	state->push_back(root->state.angularVelocity.y);
+	state->push_back(root->state.angularVelocity.z);
+
+	//now each joint introduces one more rigid body, so we'll only record its state relative to its parent.
+	//we are assuming here that each joint is revolute!!!
+	Quaternion qRel;
+	Vector3d wRel;
+
+	for (uint i=0;i<joints.size();i++){
+		getRelativeOrientation(jointOrder[i], &qRel);
+	
+		state->push_back(qRel.s);
+		state->push_back(qRel.v.x);
+		state->push_back(qRel.v.y);
+		state->push_back(qRel.v.z);
+
+		getRelativeAngularVelocity(jointOrder[i], &wRel);
+		state->push_back(wRel.x);
+		state->push_back(wRel.y);
+		state->push_back(wRel.z);
+	}
+}
+
+/**
+	This method populates the state of the current character with the values that are passed
+	in the dynamic array. The same conventions as for the getState() method are assumed.
+*/
+void Character::setState(ReducedCharacterStateArray* state, int start, bool hackFlag){
+	updateJointOrdering();
+	ReducedCharacterState rs(state, start);
+
+	//kinda ugly code....
+	root->state.position = rs.getPosition();
+	root->state.orientation = rs.getOrientation();
+	root->state.velocity = rs.getVelocity();
+	root->state.angularVelocity = rs.getAngularVelocity();
+
+	//now each joint introduces one more rigid body, so we'll only record its state relative to its parent.
+	//we are assuming here that each joint is revolute!!!
+	Quaternion qRel;
+	Vector3d wRel;
+
+	Vector3d r;
+	Vector3d d;
+	Vector3d vRel;
+
+//	root->updateToWorldTransformation();
+	for (uint j=0;j<joints.size();j++){
+		qRel = rs.getJointRelativeOrientation(j);
+		wRel = rs.getJointRelativeAngVelocity(j);
+		//transform the relative angular velocity to world coordinates
+		wRel = joints[j]->parent->getWorldCoordinates(wRel);
+
+		//now that we have this information, we need to restore the state of the rigid body.
+
+		//set the proper orientation
+		int orderedJ = j;
+		if( hackFlag )
+			orderedJ = jointOrder[j];
+
+
+		joints[orderedJ]->child->state.orientation = joints[orderedJ]->parent->state.orientation * qRel;
+		//and the proper angular velocity
+		joints[orderedJ]->child->state.angularVelocity = joints[orderedJ]->parent->state.angularVelocity + wRel;
+		//and now set the linear position and velocity
+		joints[orderedJ]->fixJointConstraints(false, true, false);
+//		joints[j]->child->updateToWorldTransformation();
+	}
+}
+
+/**
+	this method is used to return the current heading of the character, specified as an angle measured in radians
+*/
+double Character::getHeadingAngle(){
+	//first we need to get the current heading of the character. Also, note that q and -q represent the same rotation
+	Quaternion q = getHeading();
+	if (q.s<0){
+		q.s = -q.s;
+		q.v = -q.v;
+	}
+	double currentHeading = 2 * safeACOS(q.s);
+	if (q.v.dotProductWith(PhysicsGlobals::up) < 0)
+		currentHeading = -currentHeading;
+	return currentHeading;
+}
+
+
+/**
+	This method returns the dimension of the state. Note that we will consider
+	each joint as having 3-DOFs (represented by the 4 values of the quaternion)
+	without taking into account what type of joint it is (i.e. a hinge joint
+	has only one degree of freedom, but we will still consider the relative orientation
+	of the child relative to the parent as a quaternion.
+*/
+int Character::getStateDimension(){
+	//13 for the root, and 7 for every other body (and each body is introduced by a joint).
+	return 13 + 7 * joints.size();
+}
+
+
+/**
+	This method is used to mirror the given orientation. It is assumed that rotations in the sagittal plane
+	(about parent frame x-axis) are to stay the same, while the rotations about the other axes need to be reversed.
+*/
+Quaternion mirrorOrientation(const Quaternion& q){
+	//get the rotation about the parent's x-axis
+	Quaternion qSagittal = q.getComplexConjugate().decomposeRotation(Vector3d(1, 0, 0)).getComplexConjugate();
+	//this is what is left, if we removed that component of the rotation
+	Quaternion qOther = q * qSagittal.getComplexConjugate();
+	//and now negate the non-sagittal part of the rotation, but keep the rotation in the sagittal plane
+	return qOther.getComplexConjugate() * qSagittal;
+}
+
+/**
+	This method is used to multiply, element-wise, the two vectors that are passed in  
+*/
+Vector3d elemWiseMultiply(const Vector3d& a, const Vector3d& b){
+	return Vector3d(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+
+/**
+	this method takes the state of the character, passed in as an array of doubles, and reverses it inplace
+*/
+void Character::reverseStanceOfStateArray(ReducedCharacterStateArray* state, int start){
+	ReducedCharacterState chS(state, start);
+
+	char name[100];
+
+	//now we're ready to start swapping left and right body parts
+	for (uint i=0;i<joints.size();i++){
+		//get the name of the joint, and if it's a right joint, and can be matched with one on the left, swap them
+		strcpy(name, joints[jointOrder[i]]->name);
+		if (name[0] == 'r'){
+			int rJointIndex = i;
+			name[0] = 'l';
+			int lJointIndex = reverseJointOrder[this->getJointIndex(name)];
+			if (lJointIndex > 0){
+				//ok, swap the angular velocities and the orientations
+				Quaternion qr = chS.getJointRelativeOrientation(rJointIndex);
+				Quaternion ql = chS.getJointRelativeOrientation(lJointIndex);
+				chS.setJointRelativeOrientation(ql, rJointIndex);
+				chS.setJointRelativeOrientation(qr, lJointIndex);
+				Vector3d wr = chS.getJointRelativeAngVelocity(rJointIndex);
+				Vector3d wl = chS.getJointRelativeAngVelocity(lJointIndex);
+				chS.setJointRelativeAngVelocity(wl, rJointIndex);
+				chS.setJointRelativeAngVelocity(wr, lJointIndex);
+			}
+		}
+	}
+
+	//now reflect/mirror all the orientations and angular velocities,
+	//now we're ready to start swapping left and right body parts
+	for (uint i=0;i<joints.size();i++){
+		//orientations
+		chS.setJointRelativeOrientation(mirrorOrientation(chS.getJointRelativeOrientation(i)), i);
+		//angular velocities - assume that ang velocity about the x axis results in rotation in the sagittal plane, so it shouldn't be changed
+		chS.setJointRelativeAngVelocity(elemWiseMultiply(chS.getJointRelativeAngVelocity(i), Vector3d(1, -1, -1)), i);
+	}
+
+	//and now take care of the root information
+	chS.setOrientation(mirrorOrientation(chS.getOrientation()));
+	chS.setAngularVelocity(elemWiseMultiply(chS.getAngularVelocity(), Vector3d(1, -1, -1)));
+	//for the velocity, we only want the x-component reversed (i.e. movement in the sagittal plane should stay the same)
+	chS.setVelocity(elemWiseMultiply(chS.getVelocity(), Vector3d(-1, 1, 1)));	
+}
+
+/**
+	this method is used to retrieve the reduced state of the character, but with the stance reversed.
+*/
+void Character::getReverseStanceState(ReducedCharacterStateArray* state){
+	//this is the index where we will start populating the data in the state array
+	int start = state->size();
+	getState(state);
+	reverseStanceOfStateArray(state, start);
+}
+
+
+/**
+	This method is used to compute the center of mass of the articulated figure.
+*/
+Vector3d Character::getCOM(){
+	Vector3d COM = Vector3d(root->getCMPosition()) * root->getMass();
+	double curMass = root->getMass();
+	double totalMass = curMass;
+	for (uint i=0; i <joints.size(); i++){
+		curMass = joints[i]->child->getMass();
+		totalMass += curMass;
+		COM.addScaledVector(joints[i]->child->getCMPosition() , curMass);
+	}
+
+	COM /= totalMass;
+
+	return COM;
+}
+
+/**
+	This method is used to compute the velocity of the center of mass of the articulated figure.
+*/
+Vector3d Character::getCOMVelocity(){
+	Vector3d COMVel = Vector3d(root->getCMVelocity()) * root->getMass();
+	double curMass = root->getMass();
+	double totalMass = curMass;
+	for (uint i=0; i <joints.size(); i++){
+		curMass = joints[i]->child->getMass();
+		totalMass += curMass;
+		COMVel.addScaledVector(joints[i]->child->getCMVelocity() , curMass);
+	}
+
+	COMVel /= totalMass;
+
+	return COMVel;
+}
+
+/**
+	this method is used to rotate the character about the vertical axis, so that it's default heading has the value that is given as a parameter
+*/
+void Character::setHeading(Quaternion heading){
+	ReducedCharacterStateArray state;
+	getState(&state);
+	setHeading(heading, &state);
+	setState(&state);
+}
+
+
+/**
+	this method is used to rotate the character (well, the character whose state is passed in as a parameter) 
+	about the vertical axis, so that it's default heading has the value that is given as a parameter
+*/
+void Character::setHeading(Quaternion heading, ReducedCharacterStateArray* state, int start){
+	ReducedCharacterState chS(state, start);
+	Quaternion oldHeading, newHeading, qRoot;
+	//get the current root orientation, that contains information regarding the current heading
+	qRoot = chS.getOrientation();
+	//get the twist about the vertical axis...
+	oldHeading = computeHeading(qRoot);
+	//now we cancel the initial twist and add a new one of our own choosing
+	newHeading = heading * oldHeading.getComplexConjugate();
+	//add this component to the root.
+	chS.setOrientation(newHeading * qRoot);
+	//and also update the root velocity and angular velocity
+	chS.setVelocity(newHeading.rotate(chS.getVelocity()));
+	chS.setAngularVelocity(newHeading.rotate(chS.getAngularVelocity()));
+}
+
+/**
+	this method is used to rotate the character (well, the character whose state is passed in as a parameter) 
+	about the vertical axis, so that it's default heading has the value that is given as a parameter
+*/
+void Character::setHeading(double val, ReducedCharacterStateArray* state, int start){
+	setHeading(Quaternion::getRotationQuaternion(val, Vector3d(0,1,0)), state, start);
+}
+
+/**
+	this method is used to rotate the character about the vertical axis, so that it's default heading has the value that is given as a parameter
+*/
+void Character::setHeading(double val){
+	ReducedCharacterStateArray state;
+	getState(&state);
+	setHeading(Quaternion::getRotationQuaternion(val, Vector3d(0,1,0)), &state);
+	setState(&state);
+}
+
+/**
+	this method is used to rotate the character about the vertical axis, so that it's default heading has the value that is given as a parameter
+*/
+void Character::recenter(){
+	ReducedCharacterStateArray state;
+	getState(&state);
+	ReducedCharacterState chS(&state);
+	Point3d currPos = chS.getPosition();
+	currPos.x = currPos.z = 0;
+	chS.setPosition(currPos);
+	setState(&state);
+}
+
+/**
+	this method is used to return the current heading of the character
+*/
+Quaternion Character::getHeading(){
+	//get the current root orientation, that contains information regarding the current heading and retrieve the twist about the vertical axis
+	return computeHeading(root->getOrientation());
+}
+
+/**
+	this method is used to read the reduced state of the character from the file
+*/
+void Character::loadReducedStateFromFile(char* fName){
+	ReducedCharacterStateArray state;
+	readReducedStateFromFile(fName, &state);
+	setState(&state);
+}
+
+/**
+	this method is used to read the reduced state of the character from the file, into the array passed in as a parameter. The
+	state of the character is not modified
+*/
+void Character::readReducedStateFromFile(char* fName, ReducedCharacterStateArray* state){
+	// Update joint order
+	if (fName == NULL)
+		throwError("cannot write to a file whose name is NULL!");
+
+	int start = state->size();
+
+	FILE* fp = fopen(fName, "r");
+	if (fp == NULL)
+		throwError("cannot open the file \'%s\' for reading...", fName);
+
+	double temp1, temp2, temp3, temp4;
+
+	char line[100];
+
+	//read the heading first...
+	double heading;
+	readValidLine(line, fp);
+	sscanf(line, "%lf", &heading);
+	
+	readValidLine(line, fp);
+	sscanf(line, "%lf %lf %lf", &temp1, &temp2, &temp3);
+	state->push_back(temp1);
+	state->push_back(temp2);
+	state->push_back(temp3);
+	readValidLine(line, fp);
+	sscanf(line, "%lf %lf %lf %lf", &temp1, &temp2, &temp3, &temp4);
+	state->push_back(temp1);
+	state->push_back(temp2);
+	state->push_back(temp3);
+	state->push_back(temp4);
+	readValidLine(line, fp);
+	sscanf(line, "%lf %lf %lf", &temp1, &temp2, &temp3);
+	state->push_back(temp1);
+	state->push_back(temp2);
+	state->push_back(temp3);
+	readValidLine(line, fp);
+	sscanf(line, "%lf %lf %lf", &temp1, &temp2, &temp3);
+	state->push_back(temp1);
+	state->push_back(temp2);
+	state->push_back(temp3);
+
+	for (uint i=0;i<joints.size();i++){
+		readValidLine(line, fp);
+		sscanf(line, "%lf %lf %lf %lf", &temp1, &temp2, &temp3, &temp4);
+		state->push_back(temp1);
+		state->push_back(temp2);
+		state->push_back(temp3);
+		state->push_back(temp4);
+		readValidLine(line, fp);
+		sscanf(line, "%lf %lf %lf", &temp1, &temp2, &temp3);
+		state->push_back(temp1);
+		state->push_back(temp2);
+		state->push_back(temp3);
+	}
+
+	//now set the heading...
+	setHeading(Quaternion::getRotationQuaternion(heading, PhysicsGlobals::up), state, start);
+
+	fclose(fp);
+}
+
+/**
+	this method is used to write the reduced state of the character to a file
+*/
+void Character::saveReducedStateToFile(char* fName, ReducedCharacterStateArray& state){
+	// Update joint order
+	updateJointOrdering();
+
+	if (fName == NULL)
+		throwError("cannot write to a file whose name is NULL!");
+
+	FILE* fp = fopen(fName, "w");
+	if (fp == NULL)
+		throwError("cannot open the file \'%s\' for writing...", fName);
+
+	//retrieve the current heading, write it to file, and then set a zero heading for the state
+	double heading = getHeadingAngle();
+	setHeading(Quaternion::getRotationQuaternion(0, PhysicsGlobals::up), &state);
+
+	fprintf(fp, "# order is:\n# Heading\n# Position\n# Orientation\n# Velocity\n# AngularVelocity\n\n# Relative Orientation\n# Relative Angular Velocity\n#----------------\n\n# Heading\n%lf\n\n# Root(%s)\n", heading, root->name);
+	fprintf(fp, "%lf %lf %lf\n", state[0], state[1], state[2]);
+	fprintf(fp, "%lf %lf %lf %lf\n", state[3], state[4], state[5],state[6]);
+	fprintf(fp, "%lf %lf %lf\n", state[7], state[8], state[9]);
+	fprintf(fp, "%lf %lf %lf\n\n", state[10], state[11], state[12]);
+
+	for (uint i=0;i<joints.size();i++){
+		fprintf(fp, "# %s\n", joints[jointOrder[i]]->name);
+		fprintf(fp, "%lf %lf %lf %lf\n", state[13+7*i+0], state[13+7*i+1], state[13+7*i+2],state[13+7*i+3]);
+		fprintf(fp, "%lf %lf %lf\n", state[13+7*i+4], state[13+7*i+5], state[13+7*i+6]);
+		fprintf(fp, "\n");
+	}
+	fclose(fp);
+}
+
+/**
+	this method is used to write the reduced state of the character to the file
+*/
+void Character::saveReducedStateToFile(char* fName){
+	ReducedCharacterStateArray state;
+	getState(&state);
+
+	saveReducedStateToFile(fName, state);
+}
+	
+/**
+	HACK!
+*/
+void Character::updateJointOrdering(){
+	if( jointOrder.size() == joints.size() ) 
+		return; // HACK assume ordering is ok
+	jointOrder.clear();
+
+	if (!root)
+		return;
+	DynamicArray<ArticulatedRigidBody*> bodies;
+	bodies.push_back(root);
+
+	int currentBody = 0;
+
+	while ((uint)currentBody<bodies.size()){
+		//add all the children joints to the list
+		for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){
+			Joint *j = bodies[currentBody]->getChildJoint(i);
+			jointOrder.push_back( getJointIndex(j->getName()) );
+			bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild());
+		}
+		currentBody++;
+	}
+
+	reverseJointOrder.resize( jointOrder.size() );
+	for( uint i=0; i < jointOrder.size(); ++i )
+		reverseJointOrder[jointOrder[i]] = i;
+
+}
\ No newline at end of file

cartwheel-3d/Core/Character.h

+#pragma once
+
+#include <Physics/PhysicsGlobals.h>
+#include <Physics/ArticulatedFigure.h>
+#include <Utils/Utils.h>
+#include "SimGlobals.h"
+
+class ReducedCharacterStateArray : public DynamicArray<double> {
+};
+
+
+/**
+	A character is an articulated figure - This class implements methods that allow it to easily save and restore its state, etc.
+*/
+class Character : public ArticulatedFigure {
+	friend class SimBiController;
+	friend class Controller;
+	friend class IKVMCController;
+	friend class VirtualModelController;
+	friend class TestApp;
+	friend class TestApp2;
+	friend class BehaviourController;
+
+private:
+	/**
+		this method is used to rotate the character about the vertical axis, so that its heading has the value that is given as a parameter.
+		It is assumed that the quaternion passed in here represents a rotation about the vertical axis - that's why it is a private method
+	*/
+	void setHeading(Quaternion heading);
+
+	/**
+		this method is used to rotate the character (well, the character whose state is passed in as a parameter) 
+		about the vertical axis, so that it's default heading has the value that is given as a parameter.
+	*/
+	void setHeading(Quaternion heading, ReducedCharacterStateArray* state, int start = 0);
+
+
+	// Hack! This is for backward compatibility with previous formats that had the joints ordered in this way.
+	DynamicArray<int> jointOrder;
+	DynamicArray<int> reverseJointOrder;
+	void updateJointOrdering();
+
+public:
+	/**
+		the constructor
+	*/
+	Character();
+
+	/**
+		the destructor
+	*/
+	~Character(void);
+
+	/**
+		This method is used to populate the relative orientation of the parent and child bodies of joint i.
+	*/
+	void getRelativeOrientation(int i, Quaternion* qRel);
+
+	/**
+		This method is used to get the relative angular velocities of the parent and child bodies of joint i,
+		expressed in parent's local coordinates. 
+	*/
+	void getRelativeAngularVelocity(int i, Vector3d* wRel);
+
+
+	/**
+		this method is used to read the reduced state of the character from the file, into the array passed in as a parameter. The
+		state of the character is not modified
+	*/
+	void readReducedStateFromFile(char* fName, ReducedCharacterStateArray* state);
+
+	/**
+		this method is used to read the reduced state of the character from the file
+	*/
+	void loadReducedStateFromFile(char* fName);
+
+	/**
+		this method is used to write the reduced state of the character to a file
+	*/
+	void saveReducedStateToFile(char* fName);
+
+	/**
+		this method is used to write the reduced state of the character to a file
+	*/
+	void saveReducedStateToFile(char* fName, ReducedCharacterStateArray& state);
+
+	/**
+		This method populates the dynamic array passed in with the state of the character.
+		For the state, we consider the 13-dimensional state of the root, and then only
+		the relative orientation and angular velocity (as measured in parent coordinates) for
+		every other link. The velocities of the CM are derived from this information,
+		using the velocity propagation technique (if that's what it is called).		
+		The order in which the bodies appear is given by the array of joints. 
+		This works under the assumption that in the joint 
+		sequence, the parent of any rigid body appears before its children (assuming that for each joint
+		we read the parent first and then the child). 
+	*/
+	void getState(ReducedCharacterStateArray* state);
+
+	/**
+		This method populates the state of the current character with the values that are passed
+		in the dynamic array. The same conventions as for the getState() method are assumed.
+		We'll assume that the index of the first state variable in the state array is given by
+		'start'.
+	*/
+	void setState(ReducedCharacterStateArray* state, int start = 0, bool hackFlag = true);
+
+	/**
+		This method returns the dimension of the state. Note that we will consider
+		each joint as having 3-DOFs (represented by the 4 values of the quaternion)
+		without taking into account what type of joint it is (i.e. a hinge joint
+		has only one degree of freedom, but we will still consider the relative orientation
+		of the child relative to the parent as a quaternion.
+	*/
+	int getStateDimension();
+
+	/**
+		this method is used to return the current heading of the character
+	*/
+	Quaternion getHeading();
+
+	/**
+		this method is used to return the current heading of the character, specified as an angle measured in radians
+	*/
+	double getHeadingAngle();
+
+	/**
+		this method is used to rotate the character about the vertical axis, so that it's default heading has the value that is given as a parameter
+	*/
+	void setHeading(double val);
+
+	/* Recenter the character at 0,0 on the x,z plane */
+	void recenter();
+
+	/**
+		this method is used to rotate the character (well, the character whose state is passed in as a parameter) 
+		about the vertical axis, so that it's default heading has the value that is given as a parameter
+	*/
+	void setHeading(double val, ReducedCharacterStateArray* state, int start = 0);
+
+	/**
+		this method takes the state of the character, passed in as an array of doubles, and reverses it inplace
+	*/
+	void reverseStanceOfStateArray(ReducedCharacterStateArray* state, int start = 0);
+
+	/**
+		this method is used to retrieve the reduced state of the character, but with the stance reversed.
+	*/
+	void getReverseStanceState(ReducedCharacterStateArray* state);
+
+	/**
+		This method is used to compute the center of mass of the articulated figure.
+	*/
+	Vector3d getCOM();
+
+	/**
+		This method is used to compute the velocity of the center of mass of the articulated figure.
+	*/
+	Vector3d getCOMVelocity();
+
+};
+
+#define REDUCED_STATE_VAL(CHAR_STATE, I) ((*((CHAR_STATE)->state))[(I)])
+
+
+/**
+	This method decomposes the rotation that is passed in into a rotation by the vertical axis - called vertical twist or heading, and everything else:
+	rot = qHeading * qOther;
+	The returned orientation is qHeading.
+*/
+inline Quaternion computeHeading(const Quaternion& rot){
+	return rot.getComplexConjugate().decomposeRotation(PhysicsGlobals::up).getComplexConjugate();
+}
+
+
+//a useful structure that holds information regarding which joints are relevant for the distance measure, and how much they're worth
+typedef struct {
+	//this is the index of the joint, as it appears in the reduced state
+	int jIndex;
+	//this is how much the difference in the relative orientations should be worth
+	double wQ;
+	//and this is how much the difference in the relative angular velocity should be worth
+	double wV;
+} RelevantJoint;
+
+/**
+	This class is used to map the array of doubles that is used to define the state of the character to an
+	easier to use and understand meaning of the state. This should be the only place in the code where we need to know
+	that the first 13 numbers in the array represent the position, orientation, velocity and angular velocity, etc.
+*/
+class ReducedCharacterState{
+private:
+	DynamicArray<double>* state;
+	int startIndex;
+public:
+	/**
+		Constructor. Allows the index in the array of doubles to be specified as well.
+	*/
+	ReducedCharacterState(DynamicArray<double>* s, int index = 0){
+		state = s;
+		this->startIndex = index;
+	}
+
+	/**
+		Destructor - not much to do here.
+	*/
+	~ReducedCharacterState(){
+	}
+
+	/**
+		gets the root position.
+	*/
+	inline Vector3d getPosition(){
+		return Vector3d(REDUCED_STATE_VAL(this, 0+startIndex), REDUCED_STATE_VAL(this, 1+startIndex), REDUCED_STATE_VAL(this, 2+startIndex));
+	}
+
+	/**
+		sets the root position.
+	*/
+	inline void setPosition(const Vector3d& p){
+		REDUCED_STATE_VAL(this, 0 + startIndex) = p.x;
+		REDUCED_STATE_VAL(this, 1 + startIndex) = p.y;
+		REDUCED_STATE_VAL(this, 2 + startIndex) = p.z;
+	}
+
+	/**
+		gets the root orientation.
+	*/
+	inline Quaternion getOrientation(){
+		return Quaternion(REDUCED_STATE_VAL(this, 3+startIndex), REDUCED_STATE_VAL(this, 4+startIndex), REDUCED_STATE_VAL(this, 5+startIndex),  REDUCED_STATE_VAL(this, 6+startIndex));
+	}
+
+	/**
+		sets the root orientation.
+	*/
+	inline void setOrientation(const Quaternion& q){
+		REDUCED_STATE_VAL(this, 3 + startIndex) = q.s;
+		REDUCED_STATE_VAL(this, 4 + startIndex) = q.v.x;
+		REDUCED_STATE_VAL(this, 5 + startIndex) = q.v.y;
+		REDUCED_STATE_VAL(this, 6 + startIndex) = q.v.z;
+	}
+
+	/**
+		gets the root velocity.
+	*/
+	inline Vector3d getVelocity(){
+		return Vector3d(REDUCED_STATE_VAL(this, 7+startIndex), REDUCED_STATE_VAL(this, 8+startIndex), REDUCED_STATE_VAL(this, 9+startIndex));
+	}
+
+	/**
+		sets the root velocity.
+	*/
+	inline void setVelocity(const Vector3d& v){
+		REDUCED_STATE_VAL(this, 7 + startIndex) = v.x;
+		REDUCED_STATE_VAL(this, 8 + startIndex) = v.y;
+		REDUCED_STATE_VAL(this, 9 + startIndex) = v.z;
+	}
+
+	/**
+		gets the root angular velocity.
+	*/
+	inline Vector3d getAngularVelocity(){
+		return Vector3d(REDUCED_STATE_VAL(this, 10+startIndex), REDUCED_STATE_VAL(this, 11+startIndex), REDUCED_STATE_VAL(this, 12+startIndex));
+	}
+
+	/**
+		sets the root angular velocity.
+	*/
+	inline void setAngularVelocity(const Vector3d& v){
+		REDUCED_STATE_VAL(this, 10 + startIndex) = v.x;
+		REDUCED_STATE_VAL(this, 11 + startIndex) = v.y;
+		REDUCED_STATE_VAL(this, 12 + startIndex) = v.z;
+	}
+
+	/**
+		gets the relative orientation for joint jIndex
+	*/
+	inline Quaternion getJointRelativeOrientation(int jIndex){
+		int offset = startIndex + 13 + 7 * jIndex;
+		return Quaternion(REDUCED_STATE_VAL(this, 0 + offset), REDUCED_STATE_VAL(this, 1 + offset), REDUCED_STATE_VAL(this, 2 + offset),  REDUCED_STATE_VAL(this, 3 + offset));
+	}
+
+	/**
+		sets the orientation for joint jIndex
+	*/
+	inline void setJointRelativeOrientation(const Quaternion& q, int jIndex){
+		int offset = startIndex + 13 + 7 * jIndex;
+		REDUCED_STATE_VAL(this, 0 + offset) = q.s;
+		REDUCED_STATE_VAL(this, 1 + offset) = q.v.x;
+		REDUCED_STATE_VAL(this, 2 + offset) = q.v.y;
+		REDUCED_STATE_VAL(this, 3 + offset) = q.v.z;
+	}
+
+	/**
+		gets the relative angular velocity for joint jIndex
+	*/
+	inline Vector3d getJointRelativeAngVelocity(int jIndex){
+		int offset = startIndex + 13 + 7 * jIndex;
+		return Vector3d(REDUCED_STATE_VAL(this, 4 + offset), REDUCED_STATE_VAL(this, 5 + offset), REDUCED_STATE_VAL(this, 6 + offset));
+	}
+
+	/**
+		sets the orientation for joint jIndex
+	*/
+	inline void setJointRelativeAngVelocity(const Vector3d& w, int jIndex){
+		int offset = startIndex + 13 + 7 * jIndex;
+		REDUCED_STATE_VAL(this, 4 + offset) = w.x;
+		REDUCED_STATE_VAL(this, 5 + offset) = w.y;
+		REDUCED_STATE_VAL(this, 6 + offset) = w.z;
+	}
+
+
+
+
+	/**
+		this method can be used to compute the distance between two reduced character states that both belong to the raptor model.
+	*/
+	inline double raptor_computeDistanceTo(ReducedCharacterState *other){
+		//these are the settings for bip3Dv3...
+		//the joints are: body_neck, lHip, rHip, lKnee, rKnee, neck_head, tail_1
+		RelevantJoint joints[] = {{0, 1.5, 1}, {1, 1.5, 0.1}, {2, 1.5, 0.1}, {7, 1, 0.05}, {8, 1, 0.05}, {6, 1.5, 0.05}, {5, 1, 0.05}};
+		double result = 0;
+		Quaternion qa, qb;
+		Vector3d va, vb;
+
+		//for the root orientation only consider the heading independent orientation
+		qa = computeHeading(this->getOrientation()).getComplexConjugate() * this->getOrientation();
+		qb = computeHeading(other->getOrientation()).getComplexConjugate() * other->getOrientation();
+
+		va = this->getOrientation().getComplexConjugate().rotate(this->getVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getVelocity());
+
+		result += 3 * distanceBetweenOrientations(qa, qb);
+		result += 3 * (va-vb).dotProductWith(va-vb);
+
+		//and now penalize the difference in root angular velocities
+		va = this->getOrientation().getComplexConjugate().rotate(this->getAngularVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getAngularVelocity());
+		result += 3 * (va-vb).dotProductWith(va-vb);
+
+		//now go through all the joints that are relevant, and penalize the differences in the orientations and angular velocities
+		for (int i=0;i<sizeof(joints)/sizeof(joints[1]);i++){
+			qa = this->getJointRelativeOrientation(joints[i].jIndex);
+			qb = other->getJointRelativeOrientation(joints[i].jIndex);
+			va = this->getJointRelativeAngVelocity(joints[i].jIndex);
+			vb = other->getJointRelativeAngVelocity(joints[i].jIndex);
+			result += joints[i].wQ * distanceBetweenOrientations(qa, qb);
+			result += joints[i].wV * (va-vb).dotProductWith(va-vb);
+		}
+
+		return result;
+	}
+
+	/**
+		this method can be used to compute the distance between two reduced character states that both belong to the bip3dv3 model.
+	*/
+	inline double bigBird3d_computeDistanceTo(ReducedCharacterState *other){
+		//these are the settings for bip3Dv3...
+		//the joints are: body_neck, lHip, rHip, lKnee, rKnee, neck_head
+		RelevantJoint joints[] = {{0, 1.5, 1}, {1, 1.5, 0.1}, {2, 1.5, 0.1}, {4, 1, 0.05}, {5, 1, 0.05}, {3, 1.5, 0.05}};
+		double result = 0;
+		Quaternion qa, qb;
+		Vector3d va, vb;
+
+		//for the root orientation only consider the heading independent orientation
+		qa = computeHeading(this->getOrientation()).getComplexConjugate() * this->getOrientation();
+		qb = computeHeading(other->getOrientation()).getComplexConjugate() * other->getOrientation();
+
+		va = this->getOrientation().getComplexConjugate().rotate(this->getVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getVelocity());
+
+		result += 3 * distanceBetweenOrientations(qa, qb);
+		result += 3 * (va-vb).dotProductWith(va-vb);
+
+		//and now penalize the difference in root angular velocities
+		va = this->getOrientation().getComplexConjugate().rotate(this->getAngularVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getAngularVelocity());
+		result += 3 * (va-vb).dotProductWith(va-vb);
+
+		//now go through all the joints that are relevant, and penalize the differences in the orientations and angular velocities
+		for (int i=0;i<sizeof(joints)/sizeof(joints[1]);i++){
+			qa = this->getJointRelativeOrientation(joints[i].jIndex);
+			qb = other->getJointRelativeOrientation(joints[i].jIndex);
+			va = this->getJointRelativeAngVelocity(joints[i].jIndex);
+			vb = other->getJointRelativeAngVelocity(joints[i].jIndex);
+			result += joints[i].wQ * distanceBetweenOrientations(qa, qb);
+			result += joints[i].wV * (va-vb).dotProductWith(va-vb);
+		}
+
+		return result;
+	}
+
+
+
+	/**
+		this method can be used to compute the distance between two reduced character states that both belong to the bip3dv3 model.
+	*/
+	inline double bip3dv2_computeDistanceTo(ReducedCharacterState *other){
+		//these are the settings for bip3Dv3...
+		//the joints are: pelvis_torso, lHip, rHip, lKnee, rKnee
+		RelevantJoint joints[] = {{0, 1.5, 1}, {1, 1.5, 0.5}, {2, 1.5, 0.5}, {6, 1, 0.2}, {7, 1, 0.2}};
+		double result = 0;
+		Quaternion qa, qb;
+		Vector3d va, vb;
+
+		qa = computeHeading(this->getOrientation()).getComplexConjugate() * this->getOrientation();
+		qb = computeHeading(other->getOrientation()).getComplexConjugate() * other->getOrientation();
+
+		va = this->getOrientation().getComplexConjugate().rotate(this->getVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getVelocity());
+
+		result += 3 * distanceBetweenOrientations(qa, qb);
+		result += 3 * (va-vb).dotProductWith(va-vb);
+
+		//and now penalize the difference in root angular velocities
+		va = this->getOrientation().getComplexConjugate().rotate(this->getAngularVelocity());
+		vb = other->getOrientation().getComplexConjugate().rotate(other->getAngularVelocity());
+		result += 3 * 0.1 * (va-vb).dotProductWith(va-vb);
+
+		//now go through all the joints that are relevant, and penalize the differences in the orientations and angular velocities
+		for (int i=0;i<sizeof(joints)/sizeof(joints[1]);i++){
+			qa = this->getJointRelativeOrientation(joints[i].jIndex);
+			qb = other->getJointRelativeOrientation(joints[i].jIndex);
+			va = this->getJointRelativeAngVelocity(joints[i].jIndex);
+			vb = other->getJointRelativeAngVelocity(joints[i].jIndex);
+			result += joints[i].wQ * distanceBetweenOrientations(qa, qb);
+			result += 0.1 * joints[i].wV * (va-vb).dotProductWith(va-vb);
+		}
+
+		return result;
+	}
+
+};

cartwheel-3d/Core/CompositeController.cpp

+#include "CompositeController.h"
+#include <Core/ConUtils.h>
+#include <Core/SimBiController.h>
+
+CompositeController::CompositeController(Character* ch, char* input) : Controller(ch){
+
+	synchronizeControllers = false;
+	//create space for the torques we will be using...
+	for (int i=0;i<jointCount;i++)
+		torques.push_back(Vector3d());
+
+	//now we'll interpret the input file...
+	if (input == NULL)
+		throwError("NULL file name provided.");
+	FILE *f = fopen(input, "r");
+	if (f == NULL)
+		throwError("Could not open file: %s", input);
+	SimBiController* con;
+
+	//have a temporary buffer used to read the file line by line...
+	char buffer[200];
+	//this is where it happens.
+	while (!feof(f)){
+		//get a line from the file...
+		fgets(buffer, 200, f);
+		if (feof(f))
+			break;
+		if (strlen(buffer)>195)
+			throwError("The input file contains a line that is longer than ~200 characters - not allowed");
+		char *line = lTrim(buffer);
+		int lineType = getConLineType(line);
+		switch (lineType) {
+			case LOAD_CON_FILE:
+				//add a new controller
+				con = new SimBiController(character);
+				con->loadFromFile(trim(line));
+				controllers.push_back(con);
+				break;
+			case CON_NOT_IMPORTANT:
+				tprintf("Ignoring input line: \'%s\'\n", line);
+				break;
+			case CON_COMMENT:
+				break;
+			case CON_CHARACTER_STATE:
+				character->loadReducedStateFromFile(trim(line));
+				break;
+			case CON_SYNCHRONIZE_CONTROLLERS:
+				synchronizeControllers = true;
+				break;
+			case CON_STARTING_STANCE:
+				//need to set the starting stance for all the controllers
+				if (strncmp(trim(line), "left", 4) == 0){
+					setStance(LEFT_STANCE);
+				}
+				else if (strncmp(trim(line), "right", 5) == 0){
+					setStance(RIGHT_STANCE);
+				}
+				else 
+					throwError("When using the \'reverseTargetOnStance\' keyword, \'left\' or \'right\' must be specified!");
+				break;
+			default:
+				throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
+		}
+	}
+	fclose(f);
+
+	primaryControllerIndex = -1;
+	secondaryControllerIndex = -1;
+}
+
+CompositeController::~CompositeController(void){
+}
+
+/**
+	This method is used to set the stance 
+*/
+void CompositeController::setStance(int newStance){
+	for (uint i=0; i<controllers.size();i++)
+		controllers[i]->setStance(newStance);
+}
+
+/**
+	This method is used to compute the torques that are to be applied at the next step.
+*/
+void CompositeController::computeTorques(DynamicArray<ContactPoint> *cfs){
+	int cCount =  (int)controllers.size();
+	SimBiController* secondaryController = (secondaryControllerIndex < 0 || secondaryControllerIndex >= cCount) ? (NULL) : controllers[secondaryControllerIndex];
+	SimBiController* primaryController = (primaryControllerIndex < 0 || primaryControllerIndex >= cCount) ? (NULL) : controllers[primaryControllerIndex];
+
+	//we need the primary controller to be valid
+	if (primaryController == NULL)
+		throwError("A primary controller needs to always be selected for a composite controller!");
+
+	//but it shouldn't be a big deal if the secondary one is not chosen
+	if (secondaryController == NULL || synchronizeControllers == false){
+		//if we don't synchronize the controllers, then we won't interpolate either, since it doesn't make sense to interpolate between
+		//controllers that, for instance, do not agree on which part of the walk cycle the character is at!!!
+		primaryController->computeTorques(cfs);
+		for (int i=0;i<jointCount;i++)
+			this->torques[i] = primaryController->torques[i];
+		return;
+	}
+
+	if (interpValue >1)
+		interpValue = 1;
+
+	if (interpValue<0)
+		interpValue = 0;
+
+	//now compute the torques for the primary and secondary controllers
+	primaryController->computeTorques(cfs);
+	secondaryController->computeTorques(cfs);
+
+	Vector3d tmp1;
+	Vector3d tmp2;
+
+	//and now interpolate between the torques of the two controllers...
+	for (int i=0;i<jointCount;i++){
+		tmp1 = primaryController->torques[i];
+		tmp1.multiplyBy(interpValue);
+		tmp2 = secondaryController->torques[i];
+		tmp2.multiplyBy(1-interpValue);
+		this->torques[i].setToSum(tmp1, tmp2);
+	}
+
+	//and that's it
+}
+
+/**
+	This method is used to advance the controller in time. It takes in a list of the contact points, since they might be
+	used to determine when to transition to a new state. This method returns -1 if the controller does not advance to a new state,
+	or the index of the state that it transitions to otherwise.
+*/
+int CompositeController::advanceInTime(double dt, DynamicArray<ContactPoint> *cfs){
+	int cCount =  (int)controllers.size();
+	SimBiController* secondaryController = (secondaryControllerIndex < 0 || secondaryControllerIndex >= cCount) ? (NULL) : controllers[secondaryControllerIndex];
+	SimBiController* primaryController = (primaryControllerIndex < 0 || primaryControllerIndex >= cCount) ? (NULL) : controllers[primaryControllerIndex];
+
+	//we need the primary controller to be valid
+	if (primaryController == NULL)
+		throwError("A primary controller needs to always be selected for a composite controller!");
+
+	//we to advance in time the main controller. However, we need to know how much the phase should advance since we may be
+	//interpolating with the secondary controller
+
+	double dtOverT = dt / primaryController->states[primaryController->FSMStateIndex]->stateTime;
+
+	if (secondaryController != NULL && synchronizeControllers == true)
+		dtOverT = dt / ((interpValue) * primaryController->states[primaryController->FSMStateIndex]->stateTime + (1-interpValue) * secondaryController->states[secondaryController->FSMStateIndex]->stateTime);	
+
+
+	//If the primary decides that it should switch to the next state, then we will force all the other controllers switch to the next state as well. 
+	//Otherwise, we will just update their phi's so that they are all in phase
+
+	int stateIndex;
+	//advance in time the main controller, and see if it decides it should switch stance/FSM state
+	bool newFSMState = ((stateIndex = primaryController->advanceInTime(primaryController->states[primaryController->FSMStateIndex]->stateTime * dtOverT, cfs)) != -1);
+	
+	//now take care of all the other controllers
+	for (uint i=0; i<controllers.size();i++){
+		controllers[i]->updateDAndV();
+		//make sure we skip the controller that we just advanced in time above
+		if (controllers[i] == primaryController)
+			continue;
+
+		//even if we're not synchronizing the FSMState/phi of the controllers, we will still make sure that the stance/groundContact is the same
+		//for all the controllers
+		controllers[i]->bodyTouchedTheGround = primaryController->isBodyInContactWithTheGround();
+		if (synchronizeControllers == false){
+			controllers[i]->setStance(primaryController->stance);
+			continue;
+		}
+
+		//if we are synchronizing the controllers, then we have to make sure they all transition at the same time, and that they all have the same phase
+		if (newFSMState)
+			controllers[i]->transitionToState(controllers[i]->states[controllers[i]->FSMStateIndex]->nextStateIndex);
+		else
+			controllers[i]->phi = primaryController->phi;
+	}
+	return stateIndex;
+}
+
+/**
+	This method is used to populate the structure that is passed in with the current state
+	of the controller;
+*/
+void CompositeController::getControllerState(CompositeControllerState* cs){
+	cs->interpValue = this->interpValue;
+	cs->primaryControllerIndex = this->primaryControllerIndex;
+	cs->secondaryControllerIndex = this->secondaryControllerIndex;
+	cs->synchronizeControllers = this->synchronizeControllers;
+
+	for (uint i=0;i<controllers.size();i++){
+		SimBiControllerState s;
+		controllers[i]->getControllerState(&s);
+		cs->controllerStates.push_back(s);
+	}
+}
+
+/**
+	This method is used to populate the state of the controller, using the information passed in the
+	structure
+*/
+void CompositeController::setControllerState(const CompositeControllerState &cs){
+	this->interpValue = cs.interpValue;
+	this->primaryControllerIndex = cs.primaryControllerIndex;
+	this->secondaryControllerIndex = cs.secondaryControllerIndex;
+	this->synchronizeControllers = cs.synchronizeControllers;
+
+	for (uint i=0;i<controllers.size();i++)
+		controllers[i]->setControllerState(cs.controllerStates[i]);
+}
+

cartwheel-3d/Core/CompositeController.h

+#pragma once
+#include <Core/Controller.h>
+#include <Core/SimBiController.h>
+#include <Physics/ContactPoint.h>
+
+
+/**
+	This structure is used to store the state of a simbicon controller. It can be used to save/load a controller's
+	states, where the state here does not refer to the states in the Finite State Machine. The state refers to the
+	phase in the current FSM state, and also the stance.
+*/
+typedef struct {
+	int primaryControllerIndex;
+	int secondaryControllerIndex;
+	double interpValue;
+	bool synchronizeControllers;
+	DynamicArray<SimBiControllerState> controllerStates;
+} CompositeControllerState;
+
+
+/**
+	This class is used as a collection of SimBiCon controllers. This class can be used to synchronize the controllers, interpolate the torques of any 
+	two controllers, or use the results of any one controller. One assumption is made however: if the controllers are to be synchronized, then they must 
+	all share the same structure - they must all have the same number of states, and each state must correspond to a similar sort of motion (for instance,
+	state 0 should be for a left-stance action, and state 1 for a right-stance action, etc). 
+
+	It is assumed that there exists some external policy that selects the controllers to be played.
+*/
+class CompositeController :	public Controller{
+friend class RLSimulationControlProcess;
+friend class TestControllerPlayer2;
+friend class RL_Bip_GTL;
+friend class RL_Bip_SSFW;
+friend class RL_BigBird_HMV;
+friend class RL_BigBird_GTP;
+friend class BigBird_Knockdown;
+
+friend class ControllerOptimizer;
+protected:
+	//this is a list of controllers that this composition framework is responsible for
+	DynamicArray<SimBiController*> controllers;
+	//there is always one main controller that is active. This controller decides when to switch controller states, etc.
+	int primaryControllerIndex;
+	//and this is the secondary controller, that is used in the interpolation
+	int secondaryControllerIndex;
+	//and this is the interpolation value. If it is 1, then the primary controller has the say. If it is 0, then the secondary one has full control,
+	//and anything in between results in interpolation at the torque level.
+	double interpValue;
+	//if this variable is set to true, then all the controllers in the collection will be synchronized (in terms of stance, phase, and FSM state switches).
+	bool synchronizeControllers;
+public:
+	//pass as a parameter the character that the torques will be applied to, and also the file that has the list of
+	//simbicon controllers to be used
+	CompositeController(Character* ch, char* input);
+	virtual ~CompositeController(void);
+
+
+	/**
+		This method is used to set the stance for all the controllers in the collection.
+	*/
+	void setStance(int newStance);
+
+	/**
+		This method is used to compute the torques that are to be applied at the next step.
+	*/
+	virtual void computeTorques(DynamicArray<ContactPoint> *cfs);
+
+	/**
+		This method is used to advance the controller in time. It takes in a list of the contact points, since they might be
+		used to determine when to transition to a new state. This method returns -1 if the controller does not advance to a new state,
+		or the index of the state that it transitions to otherwise.
+	*/
+	int advanceInTime(double dt, DynamicArray<ContactPoint> *cfs);
+
+	/**
+		This method returns the position of the CM of the stance foot, in world coordinates
+	*/
+	inline Point3d getStanceFootPos(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->getStanceFootPos();
+		return Point3d(0,0,0);
+	}
+
+	/**
+		This method returns the position of the CM of the swing foot, in world coordinates
+	*/
+	inline Point3d getSwingFootPos(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->getSwingFootPos();
+		return Point3d(0,0,0);
+	}
+
+	inline Quaternion getCharacterFrame(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->getCharacterFrame();
+		return Quaternion();
+	}
+
+	/**
+		This method is used to populate the structure that is passed in with the current state
+		of the controller;
+	*/
+	void getControllerState(CompositeControllerState* cs);
+
+	/**
+		This method is used to populate the state of the controller, using the information passed in the
+		structure
+	*/
+	void setControllerState(const CompositeControllerState &cs);
+
+	/**
+		this method returns the phase of the locomotion cycle
+	*/
+	inline double getLocomotionPhase(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->getPhase();
+		return 0;
+	}
+
+	/**
+		this method returns the stance of the character
+	*/
+	inline int getStance(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->stance;
+		return LEFT_STANCE;
+	}
+
+	/**
+		this method returns true if some part of the character other than the foot touches the ground, false otherwise
+	*/
+	inline bool isBodyInContactWithTheGround(){
+		if (primaryControllerIndex >= 0 && primaryControllerIndex < (int)controllers.size())
+			return controllers[primaryControllerIndex]->isBodyInContactWithTheGround();
+		return true;
+	}
+
+	/**
+		this method is used to return the number of controllers used in this composite controller
+	*/
+	inline int getControllerCount(){
+		return controllers.size();
+	}
+
+	/**
+		this method is used to set the control input for this composite controller
+	*/
+	inline void setControllerInput(int primaryConIndex, int secondaryConIndex, double interpValue){
+		this->primaryControllerIndex = primaryConIndex;
+		this->secondaryControllerIndex = secondaryConIndex;
+		this->interpValue = interpValue;
+	}
+
+	/**
+		this method returns a pointer to the nth simbicon controller in this composite controller framework.
+	*/
+	inline SimBiController* getController(int i){
+		if (i < 0 || i >= (int)controllers.size())
+			return NULL;
+		return controllers[i];
+	}
+
+};
+
+
+

cartwheel-3d/Core/ConUtils.cpp

+#include "ConUtils.h"
+#include <string.h>
+
+typedef struct con_key_word{
+	char keyWord[40];
+	int retVal;
+}ConKeyWord;
+
+static ConKeyWord keywords[] = {
+	{"#", CON_COMMENT},
+	{"PDParams", CON_PD_GAINS_START},
+	{"/PDParams", CON_PD_GAINS_END},
+	{"ConState", CON_STATE_START},
+	{"/ConState", CON_STATE_END},
+	{"nextState", CON_NEXT_STATE},
+	{"description", CON_STATE_DESCRIPTION},
+	{"transitionOn", CON_TRANSITION_ON},
+	{"stateStance", CON_STATE_STANCE},
+	{"startingStance", CON_STARTING_STANCE},
+	{"startAtState", CON_START_AT_STATE},
+	{"loadCharacterState", CON_CHARACTER_STATE},
+	{"time", CON_STATE_TIME},
+	{"trajectory", CON_TRAJECTORY_START},
+	{"/trajectory", CON_TRAJECTORY_END},
+	{"baseTrajectory", CON_BASE_TRAJECTORY_START},
+	{"/baseTrajectory", CON_BASE_TRAJECTORY_END},
+	{"rotationAxis", CON_ROTATION_AXIS},
+	{"reverseTargetAngleOnStance",CON_REVERSE_ANGLE_ON_STANCE},
+	{"feedbackProjectionAxis",CON_FEEDBACK_PROJECTION_AXIS},
+	{"feedback", CON_FEEDBACK_START},
+	{"/feedback", CON_FEEDBACK_END},
+	{"cdNeg", CON_CD_NEG},
+	{"cdPos", CON_CD_POS},
+	{"cd", CON_CD},
+	{"cvNegNeg", CON_CV_NEG_NEG},
+	{"cvNegPos", CON_CV_NEG_POS},
+	{"cvPosNeg", CON_CV_POS_NEG},
+	{"cvPosPos", CON_CV_POS_POS},
+	{"cv", CON_CV},
+	{"loadRBFile", LOAD_RB_FILE},
+	{"loadController", LOAD_CON_FILE},
+	{"component", CON_TRAJ_COMPONENT},
+	{"/component", CON_TRAJ_COMPONENT_END},
+	{"synchronizeControllers", CON_SYNCHRONIZE_CONTROLLERS},
+	{"loadCompositeController", CON_LOAD_COMPOSITE_CONTROLLER},
+	{"strengthTrajectory", CON_STRENGTH_TRAJECTORY_START},
+	{"/strengthTrajectory", CON_STRENGTH_TRAJECTORY_END},
+	{"characterFrameRelative", CON_CHAR_FRAME_RELATIVE},
+	{"stanceHipDamping", CON_STANCE_HIP_DAMPING},
+	{"stanceHipMaxVelocity", CON_STANCE_HIP_MAX_VELOCITY},
+	{"dMin", CON_D_MIN},
+	{"dMax", CON_D_MAX},
+	{"vMin", CON_V_MIN},
+	{"vMax", CON_V_MAX},
+	{"dTrajX", CON_D_TRAJX_START},
+	{"/dTrajX", CON_D_TRAJX_END},
+	{"dTrajZ", CON_D_TRAJZ_START},
+	{"/dTrajZ", CON_D_TRAJZ_END},
+	{"vTrajX", CON_V_TRAJX_START},
+	{"/vTrajX", CON_V_TRAJX_END},
+	{"vTrajZ", CON_V_TRAJZ_START},
+	{"/vTrajZ", CON_V_TRAJZ_END},
+	{"rootPredictiveTorqueScale", CON_ROOT_PRED_TORQUE_SCALE},
+	{"minFeedback", CON_MIN_FEEDBACK},
+	{"maxFeedback", CON_MAX_FEEDBACK},
+	{"dScaleTraj", CON_D_SCALE_TRAJECTORY_START},
+	{"/dScaleTraj", CON_D_SCALE_TRAJECTORY_END},
+	{"vScaleTraj", CON_V_SCALE_TRAJECTORY_START},
+	{"/vScaleTraj", CON_V_SCALE_TRAJECTORY_END}
+};
+
+/**
+	This method is used to determine the type of a line that was used in the input file for a rigid body.
+	It is assumed that there are no white spaces at the beginning of the string that is passed in. the pointer buffer
+	will be updated to point at the first character after the keyword.
+*/
+int getConLineType(char* &buffer){
+
+
+	//declare a list of keywords
+	int keyWordCount = sizeof(keywords)/sizeof(keywords[0]);
+
+	if (buffer[0] == '\0')
+		return CON_COMMENT;
+
+	for (int i=0;i<keyWordCount;i++){
+		if (strncmp(buffer, keywords[i].keyWord, strlen(keywords[i].keyWord)) == 0){
+			buffer += strlen(keywords[i].keyWord);
+			return keywords[i].retVal;
+		}
+	}
+
+	return CON_NOT_IMPORTANT;
+}
+
+/**
+	This method is used to determine the string corresponding to a specific line keyword
+*/
+const char* getConLineString(int lineType){
+
+	//declare a list of keywords
+	int keyWordCount = sizeof(keywords)/sizeof(keywords[0]);
+
+	for (int i=0;i<keyWordCount;i++){
+		if ( keywords[i].retVal == lineType ){
+			return keywords[i].keyWord;
+		}
+	}
+
+	return "ERROR! Unknown lineType";
+}

cartwheel-3d/Core/ConUtils.h

+#pragma once
+
+#define CON_NOT_IMPORTANT				1
+#define CON_COMMENT						2
+#define CON_PD_GAINS_START				3
+#define CON_PD_GAINS_END				4
+#define CON_STATE_START					5
+#define CON_STATE_END					6
+#define CON_NEXT_STATE					7
+#define CON_STATE_DESCRIPTION			8
+#define CON_TRANSITION_ON				9
+#define CON_STATE_STANCE				10
+#define CON_STARTING_STANCE				11
+#define CON_START_AT_STATE				12
+#define CON_CHARACTER_STATE				13
+#define CON_STATE_TIME					14
+#define CON_TRAJECTORY_START			15
+#define CON_TRAJECTORY_END				16
+#define CON_REVERSE_ANGLE_ON_STANCE		17
+#define CON_ROTATION_AXIS				18
+#define CON_BASE_TRAJECTORY_START		19
+#define CON_BASE_TRAJECTORY_END			20
+#define CON_FEEDBACK_START				21
+#define CON_FEEDBACK_END				22
+#define CON_CD							23
+#define CON_CV							24
+#define CON_FEEDBACK_PROJECTION_AXIS    25
+#define LOAD_RB_FILE					26
+#define LOAD_CON_FILE					27
+#define CON_TRAJ_COMPONENT				28
+#define CON_TRAJ_COMPONENT_END			29
+
+#define CON_SYNCHRONIZE_CONTROLLERS		30
+#define CON_LOAD_COMPOSITE_CONTROLLER	31
+
+#define CON_STRENGTH_TRAJECTORY_START	32
+#define CON_STRENGTH_TRAJECTORY_END		33
+#define CON_CHAR_FRAME_RELATIVE         34
+#define CON_STANCE_HIP_DAMPING			35
+#define CON_STANCE_HIP_MAX_VELOCITY     36
+
+#define CON_D_MIN						37
+#define CON_D_MAX						38
+#define CON_V_MIN						39
+#define CON_V_MAX						40
+
+#define CON_D_TRAJX_START				41
+#define CON_D_TRAJX_END					42
+#define CON_D_TRAJZ_START				43
+#define CON_D_TRAJZ_END					44
+#define CON_V_TRAJX_START				45
+#define CON_V_TRAJX_END					46
+#define CON_V_TRAJZ_START				47
+#define CON_V_TRAJZ_END					48
+#define CON_ROOT_PRED_TORQUE_SCALE		49
+
+#define CON_CD_NEG						50
+#define CON_CD_POS						51
+#define CON_CV_NEG_POS					52
+#define CON_CV_NEG_NEG					53
+#define CON_CV_POS_NEG					54
+#define CON_CV_POS_POS					55
+#define CON_MAX_FEEDBACK				56
+#define CON_MIN_FEEDBACK				57
+
+#define CON_D_SCALE_TRAJECTORY_START	58
+#define CON_D_SCALE_TRAJECTORY_END		59
+
+#define CON_V_SCALE_TRAJECTORY_START	60
+#define CON_V_SCALE_TRAJECTORY_END		61
+
+
+/**
+	This method is used to determine the type of a line that was used in the input file for a rigid body.
+	It is assumed that there are no white spaces at the beginning of the string that is passed in. the pointer buffer
+	will be updated to point at the first character after the keyword.
+*/
+int getConLineType(char* &buffer);
+
+/**
+	This method is used to determine the string corresponding to a specific line keyword
+*/
+const char* getConLineString(int lineType);
+
+

cartwheel-3d/Core/Controller.cpp

+#include "Controller.h"
+
+/**
+	Default constructor.
+*/
+Controller::Controller(Character* ch) : Observable() {
+	this->character = ch;
+	jointCount = ch->getJointCount();
+	for (int i=0;i<jointCount;i++)
+		torques.push_back(Vector3d());
+	for (int i=0;i<jointCount;i++){
+		character->joints[i]->id = i;
+	}
+}
+
+/**
+	Default destructor.
+*/
+Controller::~Controller(void) {
+}
+
+
+/**
+	This method is used to apply the torques that are computed to the character that is controlled.
+*/
+void Controller::applyTorques(){
+
+	bool noOldTorqueInfo = false;
+	if (oldTorques.size() != torques.size()){
+		oldTorques.clear();
+		noOldTorqueInfo = true;
+	}
+
+	Vector3d tmpTorque, deltaT;
+	for (int i=0;i<jointCount;i++){
+		if (noOldTorqueInfo)
+			oldTorques.push_back(torques[i]);
+
+		deltaT.setToDifference(torques[i], oldTorques[i]);
+
+		double maxTorqueRateOfChange = 2000;
+
+		deltaT.x = (deltaT.x<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.x);
+		deltaT.x = (deltaT.x>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.x);
+		deltaT.y = (deltaT.y<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.y);
+		deltaT.y = (deltaT.y>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.y);
+		deltaT.z = (deltaT.z<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.z);
+		deltaT.z = (deltaT.z>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.z);
+
+
+		tmpTorque.setToSum(oldTorques[i], deltaT);
+
+		character->getJoint(i)->setTorque(tmpTorque);
+		oldTorques[i].setValues(tmpTorque);
+	}
+}
+
+/**
+	This method is used to reset the torques that are to be applied.
+*/
+void Controller::resetTorques(){
+	oldTorques.clear();
+	for (int i=0;i<jointCount;i++)
+		torques[i] = Vector3d(0,0,0);
+}
+
+

cartwheel-3d/Core/Controller.h

+#pragma once
+
+#include <Utils/Utils.h>
+#include "Character.h"
+#include <Physics/World.h>
+#include <Utils/Observable.h>
+
+/**
+	This class is used to provide a generic interface to a controller. A controller acts on a character - it computes torques that are
+	applied to the joints of the character. The details of how the torques are computed are left up to the classes that extend this one.
+*/
+class Controller : public Observable {
+	friend class DoubleStanceFeedback;
+	friend class IKVMCController;
+	friend class TestApp;
+	friend class TestApp2;
+protected:
+	//this is the character that the controller is acting on
+	Character* character;
+	//and this is the array of torques that will be computed in order for the character to match the desired pose - stored in world coordinates
+	DynamicArray<Vector3d> torques;
+	//these are the last torques that were applied - used to place limits on how fast the torques are allowed to change
+	DynamicArray<Vector3d> oldTorques;
+	//this is the number of joints of the character - stored here for easy access
+	int jointCount;
+	//This is a name for the controller
+	char name[100];
+public:
+	Controller(Character* ch);
+	virtual ~Controller(void);
+
+	void setName( const char* name ) {
+		strncpy(this->name,name,99);
+		notifyObservers();
+	}
+
+	const char* getName() {
+		return name;
+	}
+
+	Character* getCharacter() const {
+		return character;
+	}
+
+	virtual void performPreTasks(double dt, DynamicArray<ContactPoint> *cfs) {
+		computeTorques(cfs);
+		applyTorques();
+	}
+
+	// Returns true if it transitioned to a new state, false otherwise
+	virtual bool performPostTasks(double dt, DynamicArray<ContactPoint> *cfs) { return false; }
+
+	/**
+		This method is used to compute the torques, based on the current and desired poses
+	*/
+	virtual void computeTorques(DynamicArray<ContactPoint> *cfs) = 0;
+
+	/**
+		This method is used to apply the torques that are computed to the character that is controlled.
+	*/
+	void applyTorques();
+
+	/**
+		This method is used to reset the torques that are to be applied.
+	*/
+	void resetTorques();
+
+
+};

cartwheel-3d/Core/ControllerPerturbator.cpp

+#include "ControllerPerturbator.h"
+#include <string.h>
+#include <Utils/Utils.h>
+
+
+//constructor...
+Perturbation::Perturbation(double* var, char *PName, double w){
+	if (var == NULL)
+		throwError("Exception - Don't pass in a NULL value...\n");
+	variables.push_back(var);
+	originalValue = *var;
+	strcpy(name, PName);
+	this->weight = w;
+}
+
+//a copy constructor
+Perturbation::Perturbation(const Perturbation& other){
+	this->variables = other.variables;
+	this->originalValue = other.originalValue;
+	strcpy(this->name, other.name);
+	this->weight = other.weight;
+}
+
+//and a copy operator
+Perturbation& Perturbation::operator = (const Perturbation& other){
+	this->variables = other.variables;
+	this->originalValue = other.originalValue;
+	strcpy(this->name, other.name);
+	return *this;
+}
+
+void Perturbation::addVariable(double* newVar){
+	if (newVar == NULL)
+		throwError("Exception - Don't pass in a NULL value...\n");
+	if (*newVar!=originalValue)
+		throwError("Exception - The variable passed in doesn't match the existing variables.");
+	variables.push_back(newVar);
+}
+
+//this method sets the value passed in to all the variables that need to be perturbed
+void Perturbation::setValue(double newValue){
+	for (uint i=0;i<variables.size();i++)
+		*(variables[i]) = newValue;
+}
+
+//this method returns the value of the variables that are being altered
+double Perturbation::getValue(){
+	return *(variables[0]);
+}
+
+//and this method returns the default value
+double Perturbation::getDefaultValue(){
+	return originalValue;
+}
+
+//this method restores the default value of all the variables
+void Perturbation::resetVariables(){
+	setValue(this->originalValue);
+}
+
+char* Perturbation::getName(){
+	return name;
+}
+
+//constructor..
+ControllerPerturbator::ControllerPerturbator(void){
+}
+
+//destructor..
+ControllerPerturbator::~ControllerPerturbator(void){
+	for (uint i=0;i<perturbations.size();i++)
+		delete perturbations[i];
+}
+
+//resets the perturbations
+void ControllerPerturbator::resetPerturbations(){
+	for (uint i=0;i<perturbations.size();i++)
+		perturbations[i]->resetVariables();
+}
+
+//applys a given perturbation to one of the variables being altered
+void ControllerPerturbator::perturb(int which, double value){
+	if (which>=0 && (uint)which<perturbations.size())
+		perturbations[which]->setValue(value);
+}
+
+//adds a new perturbation to the collection...
+void ControllerPerturbator::addPerturbation(DynamicArray<double*> *vars, char* PName, double w){
+	if (vars->size()<=0)
+		return;
+	Perturbation* p = new Perturbation((*vars)[0], PName, w);
+	for (uint i=1;i<vars->size();i++)
+		p->addVariable((*vars)[i]);
+
+	perturbations.push_back(p);
+}
+
+//adds a new perturbation to the collection...
+void ControllerPerturbator::addPerturbation(double *var, char* PName, double w){
+	Perturbation* p = new Perturbation(var, PName, w);
+
+	perturbations.push_back(p);
+}
+
+
+//returns the list of perturbation values...
+void ControllerPerturbator::getDeltaP(DynamicArray<double> *result){
+	for (uint i=0;i<perturbations.size();i++)
+		result->push_back(perturbations[i]->getValue() - perturbations[i]->getDefaultValue());
+}
+
+//sets the perturbations...
+void ControllerPerturbator::setDeltaP(DynamicArray<double> *deltaP){
+	for (uint i=0;i<deltaP->size();i++)
+		perturbations[i]->setValue(perturbations[i]->getDefaultValue() + (*deltaP)[i]);
+}
+