Merge pull request #2047 from erwincoumans/master

Add humanoid stable PD control, preliminary optional PhysX 4.0 backend for Pybullet, various fixes

Extras/InverseDynamics/btMultiBodyTreeCreator.cpp

@@ -202,15 +202,14 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
 				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
 				break;
 			case btMultibodyLink::eSpherical:
-				
 				link.joint_type = SPHERICAL;
 				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
 				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
 				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
 				// random unit vector
 				link.body_axis_of_motion(0) = 0;
-				link.body_axis_of_motion(1) = 0;
+				link.body_axis_of_motion(1) = 1;
-				link.body_axis_of_motion(2) = 1;
+				link.body_axis_of_motion(2) = 0;
 				break;
 			case btMultibodyLink::ePlanar:
 				bt_id_error_message("planar joints not implemented\n");

examples/ExampleBrowser/premake4.lua

@@ -27,7 +27,7 @@ project "App_BulletExampleBrowser"
             initOpenCL("clew")
         end
 
-        links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","BussIK", "Bullet3Common"}
+        links{"BulletExampleBrowserLib","gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","rbdl_static","LinearMath","BussIK", "Bullet3Common"}
         initOpenGL()
         initGlew()
 
@@ -133,6 +133,17 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
 		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
+		"../SharedMemory/plugins/stablePDPlugin/SpAlg.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/Shape.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/RBDModel.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/Rand.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/MathUtil.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/KinTree.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/FileUtil.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/json/json_writer.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/json/json_value.cpp",
+		"../SharedMemory/plugins/stablePDPlugin/json/json_reader.cpp",
 		"../SharedMemory/SharedMemoryCommands.h",
 		"../SharedMemory/SharedMemoryPublic.h",
 		"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
@@ -169,6 +180,7 @@ project "App_BulletExampleBrowser"
 		"../VoronoiFracture/*",
 		"../SoftDemo/*",
 		"../RollingFrictionDemo/*",
+		"../rbdl/*",
 		"../FractureDemo/*",
 		"../DynamicControlDemo/*",
 		"../Constraints/*",

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -1353,7 +1353,9 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
 		if (linkPtr)
 		{
 			m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags);
-			m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
+			
+			int uid3 = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
+			colObj->setUserIndex3(uid3);
 		}
 	}
 }

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -113,6 +113,7 @@ void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCac
 	}
 }
 
+
 void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
 {
 	cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
@@ -126,7 +127,7 @@ void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedDat
 	}
 }
 
-void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
+void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags)
 {
 	//compute the number of links, and compute parent indices array (and possibly other cached data?)
 	cache.m_totalNumJoints1 = 0;
@@ -143,7 +144,24 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
 		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
 
 		cache.m_currentMultiBodyLinkIndex = -1;  //multi body base has 'link' index -1
-		ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
+		
+		bool maintainLinkOrder  = (flags & CUF_MAINTAIN_LINK_ORDER)!=0;
+		if (maintainLinkOrder)
+		{
+			URDF2BulletCachedData cache2 = cache;
+
+			ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
+
+			for (int j=0;j<numTotalLinksIncludingBase;j++)
+			{
+				cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
+				cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
+			}
+		}else
+		{
+			ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
+		}
+
 	}
 }
 
@@ -178,12 +196,12 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
 
 btScalar tmpUrdfScaling = 2;
 
-void ConvertURDF2BulletInternal(
+btTransform ConvertURDF2BulletInternal(
 	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
 	URDF2BulletCachedData& cache, int urdfLinkIndex,
 	const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
 	bool createMultiBody, const char* pathPrefix,
-	int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut = 0)
+	int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
 {
 	B3_PROFILE("ConvertURDF2BulletInternal2");
 	//b3Printf("start converting/extracting data from URDF interface\n");
@@ -681,13 +699,55 @@ void ConvertURDF2BulletInternal(
 
 	int numChildren = urdfChildIndices.size();
 
+	if (recursive)
+	{
+		for (int i = 0; i < numChildren; i++)
+		{
+			int urdfChildLinkIndex = urdfChildIndices[i];
+
+			ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
+		}
+	}
+	return linkTransformInWorldSpace;
+}
+
+struct childParentIndex
+{
+	int m_index;
+	int m_mbIndex;
+	int m_parentIndex;
+	int m_parentMBIndex;
+	
+};
+
+void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
+{
+	childParentIndex cp;
+	cp.m_index = urdfLinkIndex;
+	int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+	cp.m_mbIndex = mbIndex;
+	cp.m_parentIndex = parentIndex;
+	int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
+	cp.m_parentMBIndex = parentMbIndex;
+
+	allIndices.push_back(cp);
+	btAlignedObjectArray<int> urdfChildIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
+	int numChildren = urdfChildIndices.size();
 	for (int i = 0; i < numChildren; i++)
 	{
 		int urdfChildLinkIndex = urdfChildIndices[i];
-
+		GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
-		ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
 	}
 }
+
+
+bool MyIntCompareFunc(childParentIndex a, childParentIndex b)
+{
+	return (a.m_index < b.m_index);
+}
+
+
 void ConvertURDF2Bullet(
 	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
 	const btTransform& rootTransformInWorldSpace,
@@ -695,13 +755,49 @@ void ConvertURDF2Bullet(
 	bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
 {
 	URDF2BulletCachedData cache;
-	InitURDF2BulletCache(u2b, cache);
+	InitURDF2BulletCache(u2b, cache, flags);
 	int urdfLinkIndex = u2b.getRootLinkIndex();
+	int rootIndex = u2b.getRootLinkIndex();
 	B3_PROFILE("ConvertURDF2Bullet");
 
 	UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
 
-	ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
+
+	bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0;
+	if (recursive)
+	{
+		ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+	}
+	else
+	{
+		
+		btAlignedObjectArray<btTransform> parentTransforms;
+		if (urdfLinkIndex >= parentTransforms.size())
+		{
+			parentTransforms.resize(urdfLinkIndex + 1);
+		}
+		parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
+		btAlignedObjectArray<childParentIndex> allIndices;
+		
+		GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
+		allIndices.quickSort(MyIntCompareFunc);
+
+		for (int i = 0; i < allIndices.size(); i++)
+		{
+			int urdfLinkIndex = allIndices[i].m_index;
+			int parentIndex = allIndices[i].m_parentIndex;
+			btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
+			btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+			if ((urdfLinkIndex+1) >= parentTransforms.size())
+			{
+				parentTransforms.resize(urdfLinkIndex + 1);
+			}
+			parentTransforms[urdfLinkIndex] = tr;
+		}
+		
+
+
+	}
 	if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
 	{
 		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -13,28 +13,7 @@ class btTransform;
 class URDFImporterInterface;
 class MultiBodyCreationInterface;
 
-//manually sync with eURDF_Flags in SharedMemoryPublic.h!
+
-enum ConvertURDFFlags
-{
-	CUF_USE_SDF = 1,
-	// Use inertia values in URDF instead of recomputing them from collision shape.
-	CUF_USE_URDF_INERTIA = 2,
-	CUF_USE_MJCF = 4,
-	CUF_USE_SELF_COLLISION = 8,
-	CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
-	CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
-	CUF_RESERVED = 64,
-	CUF_USE_IMPLICIT_CYLINDER = 128,
-	CUF_GLOBAL_VELOCITIES_MB = 256,
-	CUF_MJCF_COLORS_FROM_FILE = 512,
-	CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
-	CUF_ENABLE_SLEEPING = 2048,
-	CUF_INITIALIZE_SAT_FEATURES = 4096,
-	CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
-	CUF_PARSE_SENSORS = 16384,
-	CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
-	CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738,
-};
 
 struct UrdfVisualShapeCache
 {

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -81,6 +81,11 @@ public:
 		return 0;
 	}
 
+	virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const
+	{
+		return 0;
+	}
+	
 	virtual int getNumAllocatedCollisionShapes() const { return 0; }
 	virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
 	virtual int getNumModels() const { return 0; }

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -2,6 +2,7 @@
 #define URDF_JOINT_TYPES_H
 
 #include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
 
 enum UrdfJointTypes
 {
@@ -76,4 +77,28 @@ struct UrdfMaterialColor
 	}
 };
 
+//manually sync with eURDF_Flags in SharedMemoryPublic.h!
+enum ConvertURDFFlags
+{
+	CUF_USE_SDF = 1,
+	// Use inertia values in URDF instead of recomputing them from collision shape.
+	CUF_USE_URDF_INERTIA = 2,
+	CUF_USE_MJCF = 4,
+	CUF_USE_SELF_COLLISION = 8,
+	CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
+	CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
+	CUF_RESERVED = 64,
+	CUF_USE_IMPLICIT_CYLINDER = 128,
+	CUF_GLOBAL_VELOCITIES_MB = 256,
+	CUF_MJCF_COLORS_FROM_FILE = 512,
+	CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
+	CUF_ENABLE_SLEEPING = 2048,
+	CUF_INITIALIZE_SAT_FEATURES = 4096,
+	CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
+	CUF_PARSE_SENSORS = 16384,
+	CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
+	CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
+	CUF_MAINTAIN_LINK_ORDER = 131072,
+};
+
 #endif  //URDF_JOINT_TYPES_H

examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h

@@ -15,7 +15,7 @@ struct UrdfRenderingInterface
 	virtual ~UrdfRenderingInterface() {}
 	///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
 	///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
 
 	///remove a visual shapes, based on the shape unique id (shapeUid)
 	virtual void removeVisualShape(int collisionObjectUid) = 0;

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -57,6 +57,7 @@ struct SimpleInternalData
 
 	int m_droidRegular;
 	int m_droidRegular2;
+	int m_textureId;
 
 	const char* m_frameDumpPngFileName;
 	FILE* m_ffmpegFile;
@@ -74,6 +75,7 @@ struct SimpleInternalData
 		  m_renderCallbacks2(0),
 		  m_droidRegular(0),
 		  m_droidRegular2(0),
+		  m_textureId(-1),
 		  m_frameDumpPngFileName(0),
 		  m_ffmpegFile(0),
 		  m_renderTexture(0),
@@ -772,6 +774,51 @@ void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], f
 
 int SimpleOpenGL3App::registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lod, int textureId)
 {
+
+	int red = 255;
+	int green = 0;
+	int blue = 128;
+	if (textureId<0)
+	{
+		if (m_data->m_textureId < 0)
+		{
+			int texWidth = 1024;
+			int texHeight = 1024;
+			b3AlignedObjectArray<unsigned char> texels;
+			texels.resize(texWidth * texHeight * 3);
+			for (int i = 0; i < texWidth * texHeight * 3; i++)
+				texels[i] = 255;
+
+			for (int i = 0; i < texWidth; i++)
+			{
+				for (int j = 0; j < texHeight; j++)
+				{
+					int a = i < texWidth / 2 ? 1 : 0;
+					int b = j < texWidth / 2 ? 1 : 0;
+
+					if (a == b)
+					{
+						texels[(i + j * texWidth) * 3 + 0] = red;
+						texels[(i + j * texWidth) * 3 + 1] = green;
+						texels[(i + j * texWidth) * 3 + 2] = blue;
+						//					texels[(i+j*texWidth)*4+3] = 255;
+					}
+					/*else
+					{
+					texels[i*3+0+j*texWidth] = 255;
+					texels[i*3+1+j*texWidth] = 255;
+					texels[i*3+2+j*texWidth] = 255;
+					}
+					*/
+				}
+			}
+
+			m_data->m_textureId = m_instancingRenderer->registerTexture(&texels[0], texWidth, texHeight);
+			
+		}
+		textureId = m_data->m_textureId;
+	}
+
 	int strideInBytes = 9 * sizeof(float);
 
 	int graphicsShapeIndex = -1;
@@ -795,17 +842,17 @@ int SimpleOpenGL3App::registerGraphicsUnitSphereShape(EnumSphereLevelOfDetail lo
 		}
 		case SPHERE_LOD_MEDIUM:
 		{
-			int numVertices = sizeof(medium_sphere_vertices) / strideInBytes;
+			int numVertices = sizeof(textured_detailed_sphere_vertices) / strideInBytes;
-			int numIndices = sizeof(medium_sphere_indices) / sizeof(int);
+			int numIndices = sizeof(textured_detailed_sphere_indices) / sizeof(int);
-			graphicsShapeIndex = m_instancingRenderer->registerShape(&medium_sphere_vertices[0], numVertices, medium_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId);
+			graphicsShapeIndex = m_instancingRenderer->registerShape(&textured_detailed_sphere_vertices[0], numVertices, textured_detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId);
 			break;
 		}
 		case SPHERE_LOD_HIGH:
 		default:
 		{
-			int numVertices = sizeof(detailed_sphere_vertices) / strideInBytes;
+			int numVertices = sizeof(textured_detailed_sphere_vertices) / strideInBytes;
-			int numIndices = sizeof(detailed_sphere_indices) / sizeof(int);
+			int numIndices = sizeof(textured_detailed_sphere_indices) / sizeof(int);
-			graphicsShapeIndex = m_instancingRenderer->registerShape(&detailed_sphere_vertices[0], numVertices, detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId);
+			graphicsShapeIndex = m_instancingRenderer->registerShape(&textured_detailed_sphere_vertices[0], numVertices, textured_detailed_sphere_indices, numIndices, B3_GL_TRIANGLES, textureId);
 			break;
 		}
 	};

examples/OpenGLWindow/Win32Window.cpp

@@ -530,11 +530,19 @@ void Win32Window::setWindowTitle(const char* titleChar)
 {
 #ifdef _WIN64
 	SetWindowTextA(m_data->m_hWnd, titleChar);
+#else
+#ifdef UNICODE
+	DWORD dwResult;
+	SendMessageTimeoutA(m_data->m_hWnd, WM_SETTEXT, 0,
+		reinterpret_cast<LPARAM>(titleChar),
+		SMTO_ABORTIFHUNG, 2000, &dwResult);
 #else
 	DWORD dwResult;
 	SendMessageTimeout(m_data->m_hWnd, WM_SETTEXT, 0,
-					   reinterpret_cast<LPARAM>(titleChar),
+		reinterpret_cast<LPARAM>(titleChar),
-					   SMTO_ABORTIFHUNG, 2000, &dwResult);
+		SMTO_ABORTIFHUNG, 2000, &dwResult);
+	
+#endif
 #endif
 }
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -903,6 +903,22 @@ B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommand
 	return 0;
 }
 
+B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex+dof] = forces[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -3459,6 +3475,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsC
 	{
 		command->m_userDebugDrawArgs.m_text[0] = 0;
 	}
+	
 	command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
 	command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
 	command->m_userDebugDrawArgs.m_startValue = startValue;
@@ -3477,6 +3494,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
 	command->m_type = CMD_USER_DEBUG_DRAW;
 	command->m_updateFlags = USER_DEBUG_READ_PARAMETER;
 	command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
+	command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
 	return (b3SharedMemoryCommandHandle)command;
 }
 
@@ -3505,6 +3523,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsCli
 	command->m_type = CMD_USER_DEBUG_DRAW;
 	command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
 	command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
+	command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
 	return (b3SharedMemoryCommandHandle)command;
 }
 
@@ -3517,6 +3536,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3Physics
 	b3Assert(command);
 	command->m_type = CMD_USER_DEBUG_DRAW;
 	command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
+	command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
 	return (b3SharedMemoryCommandHandle)command;
 }
 
@@ -3539,6 +3559,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsCli
 	b3Assert(command);
 	command->m_type = CMD_USER_DEBUG_DRAW;
 	command->m_updateFlags = 0;
+	command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
 	return (b3SharedMemoryCommandHandle)command;
 }
 
@@ -4377,7 +4398,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
 	command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
 	command->m_updateFlags = 0;
 	command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
-	
+	command->m_calculateInverseDynamicsArguments.m_flags = 0;
+
 	int dofCount = b3ComputeDofCount(physClient, bodyUniqueId);
 	
 	for (int i = 0; i < dofCount; i++)
@@ -4405,6 +4427,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2
 	command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
 	command->m_updateFlags = 0;
 	command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
+	command->m_calculateInverseDynamicsArguments.m_flags = 0;
 
 	command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ;
 	for (int i = 0; i < dofCountQ; i++)
@@ -4422,6 +4445,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2
 	return (b3SharedMemoryCommandHandle)command;
 }
 
+B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	command->m_calculateInverseDynamicsArguments.m_flags = flags;
+}
+
 B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
 														int* bodyUniqueId,
 														int* dofCount,
@@ -4511,8 +4540,13 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
 
 	return true;
 }
+B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	command->m_calculateMassMatrixArguments.m_flags = flags;
+}
 
-B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
 	b3Assert(cl);
@@ -4522,11 +4556,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
 
 	command->m_type = CMD_CALCULATE_MASS_MATRIX;
 	command->m_updateFlags = 0;
-	int numJoints = cl->getNumJoints(bodyUniqueId);
+	
-	for (int i = 0; i < numJoints; i++)
+	for (int i = 0; i < dofCountQ; i++)
 	{
 		command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
 	}
+	command->m_calculateMassMatrixArguments.m_bodyUniqueId = bodyUniqueId;
+	command->m_calculateMassMatrixArguments.m_dofCountQ = dofCountQ;
+	command->m_calculateMassMatrixArguments.m_flags = 0;
 
 	return (b3SharedMemoryCommandHandle)command;
 }
@@ -5360,3 +5397,63 @@ B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], cons
 	outOrn[3] = dorn[3];
 }
 
+b3Scalar b3GetMatrixElem(const b3Matrix3x3& mat, int index)
+{
+	int i = index % 3;
+	int j = index / 3;
+	return mat[i][j];
+}
+
+
+static bool MyMatrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
+{
+	// rot =  cy*cz          -cy*sz           sy
+	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+	b3Scalar fi = b3GetMatrixElem(mat, 2);
+	if (fi < b3Scalar(1.0f))
+	{
+		if (fi > b3Scalar(-1.0f))
+		{
+			xyz[0] = b3Atan2(-b3GetMatrixElem(mat, 5), b3GetMatrixElem(mat, 8));
+			xyz[1] = b3Asin(b3GetMatrixElem(mat, 2));
+			xyz[2] = b3Atan2(-b3GetMatrixElem(mat, 1), b3GetMatrixElem(mat, 0));
+			return true;
+		}
+		else
+		{
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
+			xyz[1] = -B3_HALF_PI;
+			xyz[2] = b3Scalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
+		xyz[1] = B3_HALF_PI;
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+
+B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/])
+{
+	b3Quaternion currentQuat(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
+	b3Quaternion desiredQuat(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
+
+	b3Quaternion relRot = currentQuat.inverse() * desiredQuat;
+	b3Vector3 angleDiff;
+	MyMatrixToEulerXYZ(b3Matrix3x3(relRot), angleDiff);
+	axisOut[0] = angleDiff[0];
+	axisOut[1] = angleDiff[1];
+	axisOut[2] = angleDiff[2];
+
+}
+
+
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -386,6 +386,7 @@ extern "C"
 																					const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
 		const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot);
+	B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
 
 	B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
 															int* bodyUniqueId,
@@ -398,7 +399,8 @@ extern "C"
 										  double* linearJacobian,
 										  double* angularJacobian);
 
-	B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ);
+	B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ);
+	B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
 	///the mass matrix is stored in column-major layout of size dofCount*dofCount
 	B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix);
 
@@ -452,7 +454,10 @@ extern "C"
 	///Only use when controlMode is CONTROL_MODE_VELOCITY
 	B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
 	B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount);
+
 	B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+	B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount);
+	
 	///Only use if when controlMode is CONTROL_MODE_TORQUE,
 	B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 
@@ -641,6 +646,7 @@ extern "C"
 	B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]);
 	B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle);
 	B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]);
+	B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/]);
 	B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]);
 	B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -67,6 +67,13 @@
 #include "plugins/pdControlPlugin/pdControlPlugin.h"
 #endif  //SKIP_STATIC_PD_CONTROL_PLUGIN
 
+
+#ifdef STATIC_LINK_SPD_PLUGIN
+#include "plugins/stablePDPlugin/BulletConversion.h"
+#include "plugins/stablePDPlugin/RBDModel.h"
+#include "plugins/stablePDPlugin/RBDutil.h"
+#endif
+
 #ifdef STATIC_LINK_VR_PLUGIN
 #include "plugins/vrSyncPlugin/vrSyncPlugin.h"
 #endif
@@ -1790,6 +1797,143 @@ struct PhysicsServerCommandProcessorInternalData
 		m_userVisualShapeHandles.initHandles();
 	}
 
+#ifdef STATIC_LINK_SPD_PLUGIN
+	b3HashMap<btHashPtr, cRBDModel*> m_rbdModels;
+
+	cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot)
+	{
+		cRBDModel* rbdModel = 0;
+		cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody);
+		if (rbdModelPtr)
+		{
+			rbdModel = *rbdModelPtr;
+		}
+		else
+		{
+			rbdModel = new cRBDModel();
+			Eigen::MatrixXd bodyDefs;
+			Eigen::MatrixXd jointMat;
+			btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat);
+			btVector3 grav = m_dynamicsWorld->getGravity();
+			tVector3 gravity(grav[0], grav[1], grav[2], 0);
+			rbdModel->Init(jointMat, bodyDefs, gravity);
+			m_rbdModels.insert(multiBody, rbdModel);
+		}
+
+		//sync pose and vel
+
+		int baseDofQ = multiBody->hasFixedBase() ? 0 : 7;
+		int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6;
+
+		Eigen::VectorXd pose, vel;
+		pose.resize(7 + multiBody->getNumPosVars());
+		vel.resize(7 + multiBody->getNumPosVars()); //??
+
+		btTransform tr = multiBody->getBaseWorldTransform();
+		int dofsrc = 0;
+		int velsrcdof = 0;
+		if (baseDofQ == 7)
+		{
+			pose[0] = jointPositionsQ[dofsrc++];
+			pose[1] = jointPositionsQ[dofsrc++];
+			pose[2] = jointPositionsQ[dofsrc++];
+
+			double quatXYZW[4];
+			quatXYZW[0] = jointPositionsQ[dofsrc++];
+			quatXYZW[1] = jointPositionsQ[dofsrc++];
+			quatXYZW[2] = jointPositionsQ[dofsrc++];
+			quatXYZW[3] = jointPositionsQ[dofsrc++];
+
+			pose[3] = quatXYZW[3];
+			pose[4] = quatXYZW[0];
+			pose[5] = quatXYZW[1];
+			pose[6] = quatXYZW[2];
+		}
+		else
+		{
+			pose[0] = tr.getOrigin()[0];
+			pose[1] = tr.getOrigin()[1];
+			pose[2] = tr.getOrigin()[2];
+			pose[3] = tr.getRotation()[3];
+			pose[4] = tr.getRotation()[0];
+			pose[5] = tr.getRotation()[1];
+			pose[6] = tr.getRotation()[2];
+		}
+		if (baseDofQdot == 6)
+		{
+			vel[0] = jointVelocitiesQdot[velsrcdof++];
+			vel[1] = jointVelocitiesQdot[velsrcdof++];
+			vel[2] = jointVelocitiesQdot[velsrcdof++];
+			vel[3] = jointVelocitiesQdot[velsrcdof++];
+			vel[4] = jointVelocitiesQdot[velsrcdof++];
+			vel[5] = jointVelocitiesQdot[velsrcdof++];
+			vel[6] = jointVelocitiesQdot[velsrcdof++];
+			vel[6] = 0;
+		}
+		else
+		{
+			vel[0] = multiBody->getBaseVel()[0];
+			vel[1] = multiBody->getBaseVel()[1];
+			vel[2] = multiBody->getBaseVel()[2];
+			vel[3] = multiBody->getBaseOmega()[0];
+			vel[4] = multiBody->getBaseOmega()[1];
+			vel[5] = multiBody->getBaseOmega()[2];
+			vel[6] = 0;
+		}
+		int dof = 7;
+		int veldof = 7;
+
+
+		for (int l = 0; l < multiBody->getNumLinks(); l++)
+		{
+			switch (multiBody->getLink(l).m_jointType)
+			{
+			case btMultibodyLink::eRevolute:
+			case btMultibodyLink::ePrismatic:
+			{
+				pose[dof++] = jointPositionsQ[dofsrc++];
+				vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			{
+				double quatXYZW[4];
+				quatXYZW[0] = jointPositionsQ[dofsrc++];
+				quatXYZW[1] = jointPositionsQ[dofsrc++];
+				quatXYZW[2] = jointPositionsQ[dofsrc++];
+				quatXYZW[3] = jointPositionsQ[dofsrc++];
+
+				pose[dof++] = quatXYZW[3];
+				pose[dof++] = quatXYZW[0];
+				pose[dof++] = quatXYZW[1];
+				pose[dof++] = quatXYZW[2];
+				vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
+				vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
+				vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
+				vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
+				break;
+			}
+			case btMultibodyLink::eFixed:
+			{
+				break;
+			}
+			default:
+			{
+				assert(0);
+			}
+			}
+		}
+
+		btVector3 gravOrg = m_dynamicsWorld->getGravity();
+		tVector grav (gravOrg[0], gravOrg[1], gravOrg[2], 0);
+		rbdModel->SetGravity(grav);
+		rbdModel->Update(pose, vel);
+
+		return rbdModel;
+	}
+
+#endif
+
 	btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
 	{
 		btInverseDynamics::MultiBodyTree* tree = 0;
@@ -3703,7 +3847,7 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
 					for (int i = 0; i < m_data->m_dynamicsWorld->getNumCollisionObjects(); i++)
 					{
 						const btCollisionObject* colObj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
-						m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getBroadphaseHandle()->getUid(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling());
+						m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getUserIndex3(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling());
 					}
 
 					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
@@ -8142,9 +8286,10 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
 			int posVarCountIndex = 7;
 			for (int i = 0; i < mb->getNumLinks(); i++)
 			{
-				bool hasPosVar = true;
+				int posVarCount = mb->getLink(i).m_posVarCount;
-
+				bool hasPosVar = posVarCount > 0;
-				for (int j = 0; j < mb->getLink(i).m_posVarCount; j++)
+								
+				for (int j = 0; j < posVarCount; j++)
 				{
 					if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
 					{
@@ -8154,7 +8299,13 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
 				}
 				if (hasPosVar)
 				{
-					mb->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
+					btQuaternion q(
+						clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
+						clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+1],
+						clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+2],
+						clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+3]);
+					q.normalize();
+					mb->setJointPosMultiDof(i, &q[0]);
 					double vel[6] = { 0, 0, 0, 0, 0, 0 };
 					mb->setJointVelMultiDof(i, vel);
 				}
@@ -8540,82 +8691,111 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
 	serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
 	if (bodyHandle && bodyHandle->m_multiBody)
 	{
-
+		if (clientCmd.m_calculateInverseDynamicsArguments.m_flags & 1)
-		btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
-
-		int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
-		int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
-		const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
-
-		if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ+ num_dofs) &&
-			clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot+ num_dofs))
 		{
-			
+#ifdef STATIC_LINK_SPD_PLUGIN
-			btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
-
-			//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
-			//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
-			if (baseDofQ)
 			{
-				btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
+				cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, 
-					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
+					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ,
-					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
+					clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot);
-
+				if (rbdModel)
-				btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
-					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
-					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
-					clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
-				btScalar yawZ, pitchY, rollX;
-				orn.getEulerZYX(yawZ, pitchY, rollX);
-				q[0] = rollX;
-				q[1] = pitchY;
-				q[2] = yawZ;
-				q[3] = pos[0];
-				q[4] = pos[1];
-				q[5] = pos[2];
-			}
-			else
-			{
-				for (int i = 0; i < num_dofs; i++)
 				{
-					q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
+					int posVal = bodyHandle->m_multiBody->getNumPosVars();
+
+					Eigen::VectorXd acc2 = Eigen::VectorXd::Zero(7 + posVal);
+					Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(7 + posVal);
+					cRBDUtil::SolveInvDyna(*rbdModel, acc2, out_tau);
+					int dof = 7 + bodyHandle->m_multiBody->getNumPosVars();
+					for (int i = 0; i < dof; i++)
+					{
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = out_tau[i];
+					}
+					serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+					serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
+					
+					serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
+					
 				}
 			}
-			for (int i = 0; i < num_dofs + baseDofQdot; i++)
+#endif
+		} else
+		{
+			btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+			int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
+			int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+			const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+
+			if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) &&
+				clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs))
 			{
-				qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
-				nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
-			}
 
-			// Set the gravity to correspond to the world gravity
+				btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
-			btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
 
-			if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+				//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
-				-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+				//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
-			{
+				if (baseDofQ)
-				serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
-				serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot;
-
-				//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
-				if (baseDofQdot)
 				{
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
+					btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
+						clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
+						clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
+
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
+					btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
+						clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
+						clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
+						clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
+					btScalar yawZ, pitchY, rollX;
+					orn.getEulerZYX(yawZ, pitchY, rollX);
+					q[0] = rollX;
+					q[1] = pitchY;
+					q[2] = yawZ;
+					q[3] = pos[0];
+					q[4] = pos[1];
+					q[5] = pos[2];
 				}
-				
+				else
-				for (int i = baseDofQdot; i < num_dofs+ baseDofQdot; i++)
 				{
-					serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
+					for (int i = 0; i < num_dofs; i++)
+					{
+						q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
+					}
+				}
+				for (int i = 0; i < num_dofs + baseDofQdot; i++)
+				{
+					qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
+					nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
+				}
+
+				// Set the gravity to correspond to the world gravity
+				btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+
+				if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+					-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
+				{
+					serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+					serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs + baseDofQdot;
+
+					//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
+					if (baseDofQdot)
+					{
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
+					}
+
+					for (int i = baseDofQdot; i < num_dofs + baseDofQdot; i++)
+					{
+						serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
+					}
+					serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
+				}
+				else
+				{
+					serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
 				}
-				serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
-			}
-			else
-			{
-				serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
 			}
 		}
 	}
@@ -8732,37 +8912,82 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
 	InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
 	if (bodyHandle && bodyHandle->m_multiBody)
 	{
-		btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+		if (clientCmd.m_calculateMassMatrixArguments.m_flags & 1)
-
-		if (tree)
 		{
-			int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+#ifdef STATIC_LINK_SPD_PLUGIN
-			const int numDofs = bodyHandle->m_multiBody->getNumDofs();
-			const int totDofs = numDofs + baseDofs;
-			btInverseDynamics::vecx q(totDofs);
-			btInverseDynamics::matxx massMatrix(totDofs, totDofs);
-			for (int i = 0; i < numDofs; i++)
 			{
-				q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
+				int posVal = bodyHandle->m_multiBody->getNumPosVars();
-			}
+				btAlignedObjectArray<double> zeroVel;
-			if (-1 != tree->calculateMassMatrix(q, &massMatrix))
+				int dof = 7 + posVal;
-			{
+				zeroVel.resize(dof);
-				serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
+				cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ,
-				// Fill in the result into the shared memory.
+					&zeroVel[0]);
-				double* sharedBuf = (double*)bufferServerToClient;
+				if (rbdModel)
-				int sizeInBytes = totDofs * totDofs * sizeof(double);
-				if (sizeInBytes < bufferSizeInBytes)
 				{
-					for (int i = 0; i < (totDofs); ++i)
+					Eigen::MatrixXd out_mass;
+					cRBDUtil::BuildMassMat(*rbdModel, out_mass);
+					int skipDofs = 0;// 7 - baseDofQ;
+					int totDofs = dof;
+					serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs-skipDofs;
+					// Fill in the result into the shared memory.
+					double* sharedBuf = (double*)bufferServerToClient;
+					int sizeInBytes = totDofs * totDofs * sizeof(double);
+					if (sizeInBytes < bufferSizeInBytes)
 					{
-						for (int j = 0; j < (totDofs); ++j)
+						for (int i = skipDofs; i < (totDofs); ++i)
 						{
-							int element = (totDofs)*i + j;
+							for (int j = skipDofs; j < (totDofs); ++j)
-
+							{
-							sharedBuf[element] = massMatrix(i, j);
+								int element = (totDofs- skipDofs)*(i- skipDofs) + (j- skipDofs);
+								double v = out_mass(i, j);
+								if (i == j && v == 0)
+								{
+									v = 1;
+								}
+								sharedBuf[element] = v;
+							}
 						}
+						serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
+					}
+				}
+			}
+#endif
+		}
+		else
+		{
+
+			btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+
+			if (tree)
+			{
+				int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+				const int numDofs = bodyHandle->m_multiBody->getNumDofs();
+				const int totDofs = numDofs + baseDofs;
+				btInverseDynamics::vecx q(totDofs);
+				btInverseDynamics::matxx massMatrix(totDofs, totDofs);
+				for (int i = 0; i < numDofs; i++)
+				{
+					q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
+				}
+				if (-1 != tree->calculateMassMatrix(q, &massMatrix))
+				{
+					serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
+					// Fill in the result into the shared memory.
+					double* sharedBuf = (double*)bufferServerToClient;
+					int sizeInBytes = totDofs * totDofs * sizeof(double);
+					if (sizeInBytes < bufferSizeInBytes)
+					{
+						for (int i = 0; i < (totDofs); ++i)
+						{
+							for (int j = 0; j < (totDofs); ++j)
+							{
+								int element = (totDofs)*i + j;
+
+								sharedBuf[element] = massMatrix(i, j);
+							}
+						}
+						serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
 					}
-					serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
 				}
 			}
 		}
@@ -9800,7 +10025,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
 											   numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
 											   &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
 					}
-					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId;
+					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
 					for (int i = 0; i < numDofs; i++)
 					{
 						serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];

examples/SharedMemory/SharedMemoryCommands.h

@@ -663,6 +663,7 @@ struct CalculateInverseDynamicsArgs
 	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
 	double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
 	double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+	int m_flags;
 };
 
 struct CalculateInverseDynamicsResultArgs
@@ -693,6 +694,8 @@ struct CalculateMassMatrixArgs
 {
 	int m_bodyUniqueId;
 	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+	int m_dofCountQ;
+	int m_flags;
 };
 
 struct CalculateMassMatrixResultArgs

examples/SharedMemory/SharedMemoryPublic.h

@@ -817,6 +817,7 @@ enum eCONNECT_METHOD
 	eCONNECT_DART = 10,
 	eCONNECT_MUJOCO = 11,
 	eCONNECT_GRPC = 12,
+	eCONNECT_PHYSX=13,
 };
 
 enum eURDF_Flags
@@ -836,6 +837,7 @@ enum eURDF_Flags
 	URDF_PARSE_SENSORS = 16384,
 	URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
 	URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
+	URDF_MAINTAIN_LINK_ORDER = 131072,
 };
 
 enum eUrdfGeomTypes  //sync with UrdfParser UrdfGeomTypes

examples/SharedMemory/physx/PhysXC_API.cpp

@@ -0,0 +1,15 @@
+#ifdef BT_ENABLE_PHYSX
+#include "PhysXC_API.h"
+#include "PhysXServerCommandProcessor.h"
+#include "PhysXClient.h"
+
+B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX()
+{
+	PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor;
+
+	PhysXClient* direct = new PhysXClient(sdk, true);
+	bool connected;
+	connected = direct->connect();
+	return (b3PhysicsClientHandle)direct;
+}
+#endif  //BT_ENABLE_PHYSX

examples/SharedMemory/physx/PhysXC_API.h

@@ -0,0 +1,20 @@
+#ifndef PHYSX_C_API_H
+#define PHYSX_C_API_H
+
+#ifdef BT_ENABLE_PHYSX
+
+#include "../PhysicsClientC_API.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //BT_ENABLE_PHYSX
+#endif  //PHYSX_C_API_H

examples/SharedMemory/physx/PhysXClient.h

@@ -0,0 +1,135 @@
+#ifndef PHYSX_CLIENT_H
+#define PHYSX_CLIENT_H
+
+#include "../PhysicsClient.h"
+
+///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
+class PhysXClient : public PhysicsClient
+{
+protected:
+	struct PhysXDirectInternalData* m_data;
+
+	bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
+
+	bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
+	bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
+
+	bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
+
+	bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
+
+	void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+
+	void processAddUserData(const struct SharedMemoryStatus& serverCmd);
+
+	void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
+
+	void resetData();
+
+	void removeCachedBody(int bodyUniqueId);
+
+public:
+	PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
+
+	virtual ~PhysXClient();
+
+	// return true if connection succesfull, can also check 'isConnected'
+	//it is OK to pass a null pointer for the gui helper
+	virtual bool connect();
+
+	////todo: rename to 'disconnect'
+	virtual void disconnectSharedMemory();
+
+	virtual bool isConnected() const;
+
+	// return non-null if there is a status, nullptr otherwise
+	virtual const SharedMemoryStatus* processServerStatus();
+
+	virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
+
+	virtual bool canSubmitCommand() const;
+
+	virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+	virtual int getNumJoints(int bodyIndex) const;
+
+	virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+
+	virtual int getNumUserConstraints() const;
+
+	virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
+
+	virtual int getUserConstraintId(int serialIndex) const;
+
+	///todo: move this out of the
+	virtual void setSharedMemoryKey(int key);
+
+	void uploadBulletFileToSharedMemory(const char* data, int len);
+
+	virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
+
+	virtual int getNumDebugLines() const;
+
+	virtual const float* getDebugLinesFrom() const;
+	virtual const float* getDebugLinesTo() const;
+	virtual const float* getDebugLinesColor() const;
+
+	virtual void getCachedCameraImage(b3CameraImageData* cameraData);
+
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
+	virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
+
+	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
+
+	virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
+
+	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
+
+	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
+
+	virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
+
+	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
+
+	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
+
+	//the following APIs are for internal use for visualization:
+	virtual bool connect(struct GUIHelperInterface* guiHelper);
+	virtual void renderScene();
+	virtual void debugDraw(int debugDrawMode);
+
+	virtual void setTimeOut(double timeOutInSeconds);
+	virtual double getTimeOut() const;
+
+	virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
+	{
+		return false;
+	}
+	virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
+	{
+		return -1;
+	}
+	virtual int getNumUserData(int bodyUniqueId) const
+	{
+		return 0;
+	}
+	virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
+	{
+		*keyOut = 0;
+		*userDataIdOut = -1;
+		return;
+	}
+
+	virtual void pushProfileTiming(const char* timingName);
+	virtual void popProfileTiming();
+};
+
+#endif  //PHYSX_CLIENT_H

examples/SharedMemory/physx/PhysXServerCommandProcessor.h

@@ -0,0 +1,47 @@
+#ifndef PHYSX_SERVER_COMMAND_PROCESSOR_H
+#define PHYSX_SERVER_COMMAND_PROCESSOR_H
+
+#include "../PhysicsCommandProcessorInterface.h"
+
+class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
+{
+	struct PhysXServerCommandProcessorInternalData* m_data;
+
+	bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+
+	bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+
+	void resetSimulation();
+	bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+public:
+	PhysXServerCommandProcessor();
+
+	virtual ~PhysXServerCommandProcessor();
+
+	virtual bool connect();
+
+	virtual void disconnect();
+
+	virtual bool isConnected() const;
+
+	virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+
+	virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+
+	virtual void renderScene(int renderFlags) {}
+	virtual void physicsDebugDraw(int debugDrawFlags) {}
+	virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {}
+	virtual void setTimeOut(double timeOutInSeconds) {}
+
+	virtual void reportNotifications() {}
+};
+
+#endif  //PHYSX_SERVER_COMMAND_PROCESSOR_H

examples/SharedMemory/physx/PhysXUrdfImporter.h

@@ -0,0 +1,116 @@
+#ifndef PHYSX_URDF_IMPORTER_H
+#define PHYSX_URDF_IMPORTER_H
+
+#include "../../Importers/ImportURDFDemo/URDFImporterInterface.h"
+#include "../../Importers/ImportURDFDemo/UrdfRenderingInterface.h"
+
+struct PhysXURDFTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData1;
+	bool m_isCached;
+};
+
+
+
+
+
+///PhysXURDFImporter can deal with URDF and SDF files
+class PhysXURDFImporter : public URDFImporterInterface
+{
+	struct PhysXURDFInternalData* m_data;
+
+public:
+	PhysXURDFImporter(struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0);
+
+	virtual ~PhysXURDFImporter();
+
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
+
+	//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
+	virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
+	virtual int getNumModels() const;
+	virtual void activateModel(int modelIndex);
+	virtual void setBodyUniqueId(int bodyId);
+	virtual int getBodyUniqueId() const;
+	const char* getPathPrefix();
+
+	void printTree();  //for debugging
+
+	virtual int getRootLinkIndex() const;
+
+	virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+
+	virtual std::string getBodyName() const;
+
+	virtual std::string getLinkName(int linkIndex) const;
+
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+
+	virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
+
+	virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
+
+	virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
+
+	virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
+
+	virtual std::string getJointName(int linkIndex) const;
+
+	virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
+	virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
+
+	virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+
+	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
+	virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
+
+	
+
+	virtual int convertLinkVisualShapes3(
+		int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
+		const struct UrdfLink* linkPtr, const UrdfModel* model,
+		int collisionObjectUniqueId, int bodyUniqueId, struct   CommonFileIOInterface* fileIO) const;
+
+	virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
+
+	class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const
+	{
+		return 0;
+	}
+
+	virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
+
+	///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
+
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
+	{
+		return 0;
+	}
+
+	virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const;
+
+	virtual const struct UrdfModel* getUrdfModel() const;
+
+	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
+
+	virtual int getNumAllocatedCollisionShapes() const;
+	virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+
+	virtual int getNumAllocatedMeshInterfaces() const;
+	virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
+
+	virtual int getNumAllocatedTextures() const;
+	virtual int getAllocatedTexture(int index) const;
+
+	virtual void setEnableTinyRenderer(bool enable);
+	//void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct PhysXURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
+
+
+	int getNumPhysXLinks() const;
+
+};
+
+#endif  //PHYSX_URDF_IMPORTER_H

examples/SharedMemory/physx/PhysXUserData.h

@@ -0,0 +1,8 @@
+#ifndef PHYSX_USER_DATA_H
+#define PHYSX_USER_DATA_H
+
+struct MyPhysXUserData
+{
+	int m_graphicsUniqueId;
+};
+#endif //PHYSX_USER_DATA_H

examples/SharedMemory/physx/URDF2PhysX.cpp

@@ -0,0 +1,908 @@
+
+#include "URDF2PhysX.h"
+
+#include "PhysXUrdfImporter.h"
+#include "Bullet3Common/b3Logging.h"
+#include "PxArticulationReducedCoordinate.h"
+#include "PxArticulationJointReducedCoordinate.h"
+#include "PxRigidActorExt.h"
+#include "PxArticulation.h"
+#include "PxRigidBodyExt.h"
+#include "PxRigidBody.h"
+#include "LinearMath/btThreads.h"
+#include "PxRigidActorExt.h"
+#include "PxArticulationBase.h"
+#include "PxArticulationLink.h"
+#include "PxMaterial.h"
+#include "PxCooking.h"
+#include "PxScene.h"
+#include "PxAggregate.h"
+
+#include "PhysXUserData.h"
+#include "../../CommonInterfaces/CommonFileIOInterface.h"
+
+#include "Importers/ImportURDFDemo/UrdfParser.h"
+
+
+
+
+struct URDF2PhysXCachedData
+{
+	URDF2PhysXCachedData()
+		: m_currentMultiBodyLinkIndex(-1),
+		m_articulation(0),
+		m_totalNumJoints1(0)
+	{
+	}
+	//these arrays will be initialized in the 'InitURDF2BulletCache'
+
+	btAlignedObjectArray<int> m_urdfLinkParentIndices;
+	btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
+	btAlignedObjectArray<class physx::PxArticulationLink*> m_urdfLink2physxLink;
+	btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
+
+	int m_currentMultiBodyLinkIndex;
+
+	physx::PxArticulationReducedCoordinate* m_articulation;
+
+	btAlignedObjectArray<physx::PxTransform> m_linkTransWorldSpace;
+	btAlignedObjectArray<int> m_urdfLinkIndex;
+	btAlignedObjectArray<int> m_parentUrdfLinkIndex;
+	btAlignedObjectArray<physx::PxReal> m_linkMasses;
+	btAlignedObjectArray<physx::PxArticulationJointType::Enum> m_jointTypes;
+
+	btAlignedObjectArray<physx::PxTransform> m_parentLocalPoses;
+	btAlignedObjectArray<physx::PxTransform> m_childLocalPoses;
+	btAlignedObjectArray<UrdfGeomTypes> m_geomTypes;
+	btAlignedObjectArray<physx::PxVec3> m_geomDimensions;
+	btAlignedObjectArray<physx::PxVec3> m_linkMaterials;
+	btAlignedObjectArray<physx::PxTransform>  m_geomLocalPoses;
+	
+
+
+	//this will be initialized in the constructor
+	int m_totalNumJoints1;
+	int getParentUrdfIndex(int linkIndex) const
+	{
+		return m_urdfLinkParentIndices[linkIndex];
+	}
+	int getMbIndexFromUrdfIndex(int urdfIndex) const
+	{
+		if (urdfIndex == -2)
+			return -2;
+		return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
+	}
+
+	void registerMultiBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal,  const btTransform& localInertialFrame)
+	{
+		m_urdfLink2physxLink[urdfLinkIndex] = body;
+		m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+	}
+
+	class physx::PxArticulationLink* getPhysxLinkFromLink(int urdfLinkIndex)
+	{
+		return m_urdfLink2physxLink[urdfLinkIndex];
+	}
+
+	void registerRigidBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
+	{
+		btAssert(m_urdfLink2physxLink[urdfLinkIndex] == 0);
+
+		m_urdfLink2physxLink[urdfLinkIndex] = body;
+		m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+	}
+};
+
+
+void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
+{
+	cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
+	cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
+
+	btAlignedObjectArray<int> childIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
+	for (int i = 0; i < childIndices.size(); i++)
+	{
+		ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
+	}
+}
+
+void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int linkIndex)
+{
+	btAlignedObjectArray<int> childIndices;
+	u2b.getLinkChildIndices(linkIndex, childIndices);
+	cache.m_totalNumJoints1 += childIndices.size();
+	for (int i = 0; i < childIndices.size(); i++)
+	{
+		int childIndex = childIndices[i];
+		ComputeTotalNumberOfJoints(u2b, cache, childIndex);
+	}
+}
+
+
+void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int flags)
+{
+	//compute the number of links, and compute parent indices array (and possibly other cached data?)
+	cache.m_totalNumJoints1 = 0;
+
+	int rootLinkIndex = u2b.getRootLinkIndex();
+	if (rootLinkIndex >= 0)
+	{
+		ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
+		int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
+
+		cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLink2physxLink.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
+
+		cache.m_currentMultiBodyLinkIndex = -1;  //multi body base has 'link' index -1
+
+		bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0;
+		if (maintainLinkOrder)
+		{
+			URDF2PhysXCachedData cache2 = cache;
+
+			ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
+
+			for (int j = 0; j<numTotalLinksIncludingBase; j++)
+			{
+				cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
+				cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
+			}
+		}
+		else
+		{
+			ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
+		}
+
+	}
+}
+
+
+struct ArticulationCreationInterface
+{
+	physx::PxFoundation* m_foundation;
+	physx::PxPhysics* m_physics;
+	physx::PxCooking* m_cooking;
+	physx::PxScene* m_scene;
+	CommonFileIOInterface* m_fileIO;
+
+	b3AlignedObjectArray<int> m_mb2urdfLink;
+	void addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
+	{
+		if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
+		{
+			m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
+		}
+		//    m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
+		m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
+	}
+};
+
+static btVector4 colors4[4] =
+{
+	btVector4(1, 0, 0, 1),
+	btVector4(0, 1, 0, 1),
+	btVector4(0, 1, 1, 1),
+	btVector4(1, 1, 0, 1),
+};
+
+static btVector4 selectColor4()
+{
+	static btSpinMutex sMutex;
+	sMutex.lock();
+	static int curColor = 0;
+	btVector4 color = colors4[curColor];
+	curColor++;
+	curColor &= 3;
+	sMutex.unlock();
+	return color;
+}
+
+
+
+
+static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx::PxVec3* verts, ArticulationCreationInterface& creation)
+{
+	physx::PxCookingParams params = creation.m_cooking->getParams();
+
+	// Use the new (default) PxConvexMeshCookingType::eQUICKHULL
+	params.convexMeshCookingType = physx::PxConvexMeshCookingType::eQUICKHULL;
+
+	// If the gaussMapLimit is chosen higher than the number of output vertices, no gauss map is added to the convex mesh data (here 256).
+	// If the gaussMapLimit is chosen lower than the number of output vertices, a gauss map is added to the convex mesh data (here 16).
+	int gaussMapLimit = 256;
+
+	params.gaussMapLimit = gaussMapLimit;
+	creation.m_cooking->setParams(params);
+
+	// Setup the convex mesh descriptor
+	physx::PxConvexMeshDesc desc;
+
+	// We provide points only, therefore the PxConvexFlag::eCOMPUTE_CONVEX flag must be specified
+	desc.points.data = verts;
+	desc.points.count = numVerts;
+	desc.points.stride = sizeof(physx::PxVec3);
+	desc.flags = physx::PxConvexFlag::eCOMPUTE_CONVEX;
+
+	physx::PxU32 meshSize = 0;
+
+	// Directly insert mesh into PhysX
+	physx::PxConvexMesh* convex = creation.m_cooking->createConvexMesh(desc, creation.m_physics->getPhysicsInsertionCallback());
+	PX_ASSERT(convex);
+
+	return convex;
+}
+
+
+int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
+	physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr)
+{
+	int numShapes = 0;
+
+	URDFLinkContactInfo contactInfo;
+	u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+	
+	//static friction, dynamic frictoin, restitution
+	cache.m_linkMaterials.push_back(physx::PxVec3(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution));
+	physx::PxMaterial* material = creation.m_physics->createMaterial(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution);
+	
+	const UrdfLink* link = u2b.getUrdfLink(urdfLinkIndex);//.convertLinkCollisionShapes m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	btAssert(linkPtr);
+	if (linkPtr)
+	{
+
+		for (int v = 0; v < link->m_collisionArray.size(); v++)
+		{
+			const UrdfCollision& col = link->m_collisionArray[v];
+			const UrdfCollision* collision = &col;
+			btTransform childTrans = col.m_linkLocalFrame;
+			btTransform localTrans = localInertiaFrame.inverse()*childTrans;
+			physx::PxTransform tr;
+			tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
+			tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
+
+			physx::PxShape* shape = 0;
+			cache.m_geomTypes.push_back(col.m_geometry.m_type);
+			
+
+			switch (col.m_geometry.m_type)
+			{
+			case URDF_GEOM_PLANE:
+			{
+				btVector3 planeNormal = col.m_geometry.m_planeNormal;
+				btScalar planeConstant = 0;  //not available?
+				//PxPlane(1, 0, 0, 0).
+				shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
+				//todo: compensate for plane axis
+				//physx::PxTransform localPose = tr*physx::PxTransform
+				//shape->setLocalPose(localPose);
+				numShapes++;
+				break;
+			}
+			case URDF_GEOM_CAPSULE:
+			{
+				btScalar radius = collision->m_geometry.m_capsuleRadius;
+				btScalar height = collision->m_geometry.m_capsuleHeight;
+
+				shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material);
+
+				btTransform childTrans = col.m_linkLocalFrame;
+				btTransform x2z;
+				x2z.setIdentity();
+				x2z.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
+				btTransform localTrans = localInertiaFrame.inverse()*childTrans*x2z;
+				physx::PxTransform tr;
+				tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
+				tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
+
+				shape->setLocalPose(tr);
+				cache.m_geomLocalPoses.push_back(tr);
+				numShapes++;
+				//btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
+				//shape = capsuleShape;
+				//shape->setMargin(gUrdfDefaultCollisionMargin);
+				break;
+			}
+
+			case URDF_GEOM_CYLINDER:
+			{
+				btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
+				btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight;
+				cache.m_geomDimensions.push_back(physx::PxVec3(cylRadius, cylHalfLength,0));
+				//if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
+				//{
+				//	btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength);
+				//	btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+				//	shape = cylZShape;
+				//}
+				//else
+				{
+					btAlignedObjectArray<physx::PxVec3> vertices;
+					
+					int numSteps = 32;
+					for (int i = 0; i < numSteps; i++)
+					{
+						physx::PxVec3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength);
+						vertices.push_back(vert);
+						vert[2] = -cylHalfLength;
+						vertices.push_back(vert);
+					}
+
+					physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation);
+
+					shape  = physx::PxRigidActorExt::createExclusiveShape(*linkPtr,
+						physx::PxConvexMeshGeometry(convexMesh), *material);
+
+					shape->setLocalPose(tr);
+					cache.m_geomLocalPoses.push_back(tr);
+					numShapes++;
+					
+				}
+				break;
+			}
+			case URDF_GEOM_BOX:
+			{
+				btVector3 extents = collision->m_geometry.m_boxSize;
+				shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxBoxGeometry(extents[0] * 0.5, extents[1] * 0.5, extents[2] * 0.5), *material);
+				cache.m_geomDimensions.push_back(physx::PxVec3(extents[0]*0.5, extents[1] * 0.5, extents[2] * 0.5));
+				shape->setLocalPose(tr);
+				cache.m_geomLocalPoses.push_back(tr);
+				numShapes++;
+				break;
+			}
+			case URDF_GEOM_SPHERE:
+			{
+				btScalar radius = collision->m_geometry.m_sphereRadius;
+				shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxSphereGeometry(radius), *material);
+				cache.m_geomDimensions.push_back(physx::PxVec3(radius,0,0));
+				shape->setLocalPose(tr);
+				cache.m_geomLocalPoses.push_back(tr);
+				numShapes++;
+				break;
+			}
+
+			default:
+			{
+			}
+			}
+
+			if (shape)
+			{
+				//see https://github.com/NVIDIAGameWorks/PhysX/issues/21
+				physx::PxReal contactOffset = shape->getContactOffset();
+				physx::PxReal restOffset = shape->getContactOffset();
+
+				//shape->setContactOffset(physx::PxReal(.03));
+				//shape->setRestOffset(physx::PxReal(.01)); //
+			}
+		}
+
+	}
+	
+	
+	return numShapes;
+}
+
+btTransform ConvertURDF2PhysXInternal(
+	const PhysXURDFImporter& u2b,
+	ArticulationCreationInterface& creation,
+	URDF2PhysXCachedData& cache, int urdfLinkIndex,
+	const btTransform& parentTransformInWorldSpace, 
+	bool createMultiBody, const char* pathPrefix,
+	int flags, 
+	UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn, 
+	UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut, 
+	bool recursive)
+{
+	B3_PROFILE("ConvertURDF2PhysXInternal");
+	//b3Printf("start converting/extracting data from URDF interface\n");
+
+	btTransform linkTransformInWorldSpace;
+	linkTransformInWorldSpace.setIdentity();
+
+	int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+
+	int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
+	int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
+	
+	
+
+	//b3Printf("mb link index = %d\n",mbLinkIndex);
+
+	btTransform parentLocalInertialFrame;
+	parentLocalInertialFrame.setIdentity();
+	btScalar parentMass(1);
+	btVector3 parentLocalInertiaDiagonal(1, 1, 1);
+
+	if (urdfParentIndex == -2)
+	{
+		//b3Printf("root link has no parent\n");
+	}
+	else
+	{
+		//b3Printf("urdf parent index = %d\n",urdfParentIndex);
+		//b3Printf("mb parent index = %d\n",mbParentIndex);
+		//parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
+		u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
+	}
+
+	btScalar mass = 0;
+	btTransform localInertialFrame;
+	localInertialFrame.setIdentity();
+	btVector3 localInertiaDiagonal(0, 0, 0);
+	u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
+
+	btTransform parent2joint;
+	parent2joint.setIdentity();
+
+	int jointType;
+	btVector3 jointAxisInJointSpace;
+	btScalar jointLowerLimit;
+	btScalar jointUpperLimit;
+	btScalar jointDamping;
+	btScalar jointFriction;
+	btScalar jointMaxForce;
+	btScalar jointMaxVelocity;
+
+	bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
+
+	btTransform axis2Reference;
+	axis2Reference.setIdentity();
+
+	switch (jointType)
+	{
+	case URDFContinuousJoint:
+	case URDFPrismaticJoint:
+	case URDFRevoluteJoint:
+	{
+		//rotate from revolute 'axis' to standard X axis
+		btVector3 refAxis(1, 0, 0);
+		btVector3 axis = jointAxisInJointSpace;
+		//btQuaternion axis2ReferenceRot(btVector3(0, 1, 0), SIMD_HALF_PI);// = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+		btQuaternion axis2ReferenceRot = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+		axis2Reference.setRotation(axis2ReferenceRot);
+		break;
+	}
+	default:
+	{
+	}
+	};
+	
+	parent2joint = parent2joint*axis2Reference;
+	//localInertialFrame = axis2Reference.inverse()*localInertialFrame;
+
+	std::string linkName = u2b.getLinkName(urdfLinkIndex);
+
+	if (flags & CUF_USE_SDF)
+	{
+		parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
+	}
+	else
+	{
+		if (flags & CUF_USE_MJCF)
+		{
+			linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
+		}
+		else
+		{
+			linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
+		}
+	}
+
+	
+	int graphicsIndex;
+	{
+		B3_PROFILE("convertLinkVisualShapes");
+
+
+		
+		graphicsIndex = u2b.convertLinkVisualShapes3(urdfLinkIndex, pathPrefix, localInertialFrame, u2b.getUrdfLink(urdfLinkIndex), u2b.getUrdfModel(), -1, u2b.getBodyUniqueId(), creation.m_fileIO);
+		
+
+#if 0
+		if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
+		{
+			graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
+			UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
+			u2b.setLinkColor2(urdfLinkIndex, matColor);
+		}
+		else
+		{
+			graphicsIndex = 
+			if (cachedLinkGraphicsShapesOut)
+			{
+				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
+				UrdfMaterialColor matColor;
+				u2b.getLinkColor2(urdfLinkIndex, matColor);
+				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
+			}
+		}
+#endif
+	}
+	
+	if (1)
+	{
+		UrdfMaterialColor matColor;
+		btVector4 color2 = selectColor4();
+		btVector3 specular(0.5, 0.5, 0.5);
+		if (u2b.getLinkColor2(urdfLinkIndex, matColor))
+		{
+			color2 = matColor.m_rgbaColor;
+			specular = matColor.m_specularColor;
+		}
+
+		/*
+		if (visual->material.get())
+		{
+		color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+		}
+		*/
+		if (mass)
+		{
+			if (!(flags & CUF_USE_URDF_INERTIA))
+			{
+				//b3Assert(0);
+				//compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
+				btAssert(localInertiaDiagonal[0] < 1e10);
+				btAssert(localInertiaDiagonal[1] < 1e10);
+				btAssert(localInertiaDiagonal[2] < 1e10);
+			}
+			URDFLinkContactInfo contactInfo;
+			u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+			//temporary inertia scaling until we load inertia from URDF
+			if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
+			{
+				localInertiaDiagonal *= contactInfo.m_inertiaScaling;
+			}
+		}
+
+		
+		btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
+		bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
+
+		physx::PxArticulationLink* linkPtr = 0;
+
+		if (!createMultiBody)
+		{
+
+		}
+		else
+		{
+			physx::PxTransform tr;
+			tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z());
+			tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w());
+
+			cache.m_linkTransWorldSpace.push_back(tr);
+			cache.m_urdfLinkIndex.push_back(urdfLinkIndex);
+			cache.m_parentUrdfLinkIndex.push_back(urdfParentIndex);
+			cache.m_linkMasses.push_back(mass);
+
+			if (cache.m_articulation == 0)
+			{
+
+				bool isFixedBase = (mass == 0);  //todo: figure out when base is fixed
+
+				cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate();
+
+				if (isFixedBase)
+				{
+					cache.m_articulation->setArticulationFlags(physx::PxArticulationFlag::eFIX_BASE);
+				}
+
+
+
+				physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr);
+				
+				linkPtr = base;
+
+				
+				//physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial);
+				physx::PxRigidBody& body = *base;
+				
+				//Now create the slider and fixed joints...
+
+				cache.m_articulation->setSolverIterationCounts(32);//todo: API?
+				
+				cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
+				cache.m_parentLocalPoses.push_back(physx::PxTransform());
+				cache.m_childLocalPoses.push_back(physx::PxTransform());
+
+				// Stabilization can create artefacts on jointed objects so we just disable it
+				//cache.m_articulation->setStabilizationThreshold(0.0f);
+				//cache.m_articulation->setMaxProjectionIterations(16);
+				//cache.m_articulation->setSeparationTolerance(0.001f);
+
+#if 0
+				int totalNumJoints = cache.m_totalNumJoints1;
+				cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
+				if (flags & CUF_GLOBAL_VELOCITIES_MB)
+				{
+					cache.m_bulletMultiBody->useGlobalVelocities(true);
+				}
+				if (flags & CUF_USE_MJCF)
+				{
+					cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
+				}
+
+				
+#endif
+			}
+			else
+			{
+
+				physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex);
+				linkPtr  = cache.m_articulation->createLink(parentLink, tr);
+				
+				physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(linkPtr->getInboundJoint());
+			
+				switch (jointType)
+				{
+					case URDFFixedJoint:
+					{
+						joint->setJointType(physx::PxArticulationJointType::eFIX);
+						break;
+					}
+					case URDFSphericalJoint:
+					{
+						joint->setJointType(physx::PxArticulationJointType::eSPHERICAL);
+						joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
+						joint->setMotion(physx::PxArticulationAxis::eSWING2, physx::PxArticulationMotion::eFREE);
+						joint->setMotion(physx::PxArticulationAxis::eSWING1, physx::PxArticulationMotion::eFREE);
+						break;
+					}
+					case URDFContinuousJoint:
+					case URDFRevoluteJoint:
+					{
+						joint->setJointType(physx::PxArticulationJointType::eREVOLUTE);
+						joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
+						
+						break;
+					}
+					case URDFPrismaticJoint:
+					{
+						joint->setJointType(physx::PxArticulationJointType::ePRISMATIC);
+						joint->setMotion(physx::PxArticulationAxis::eX, physx::PxArticulationMotion::eFREE);
+						break;
+					}
+					default:
+					{
+						joint->setJointType(physx::PxArticulationJointType::eFIX);
+						btAssert(0);
+					}
+				};
+				
+				cache.m_jointTypes.push_back(joint->getJointType());
+				btTransform offsetInA, offsetInB;
+
+				offsetInA = parentLocalInertialFrame.inverse() *   parent2joint;
+				offsetInB = (axis2Reference.inverse()*localInertialFrame).inverse();
+
+				physx::PxTransform parentPose(physx::PxVec3(offsetInA.getOrigin()[0], offsetInA.getOrigin()[1], offsetInA.getOrigin()[2]),
+					physx::PxQuat(offsetInA.getRotation()[0], offsetInA.getRotation()[1], offsetInA.getRotation()[2], offsetInA.getRotation()[3]));
+
+				physx::PxTransform childPose(physx::PxVec3(offsetInB.getOrigin()[0], offsetInB.getOrigin()[1], offsetInB.getOrigin()[2]),
+					physx::PxQuat(offsetInB.getRotation()[0], offsetInB.getRotation()[1], offsetInB.getRotation()[2], offsetInB.getRotation()[3]));
+
+				cache.m_parentLocalPoses.push_back(parentPose);
+				cache.m_childLocalPoses.push_back(childPose);
+
+				joint->setParentPose(parentPose);
+				joint->setChildPose(childPose);
+				
+			}
+			cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
+			if (linkPtr)
+			{
+				//todo: mem leaks
+				MyPhysXUserData* userData = new MyPhysXUserData();
+				userData->m_graphicsUniqueId = graphicsIndex;
+				linkPtr->userData = userData;
+			}
+		}
+
+
+		//create collision shapes
+
+		//physx::PxRigidActorExt::createExclusiveShape
+		convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
+
+		
+		physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass);
+		
+		
+		//base->setMass(massOut);
+		//base->setMassSpaceInertiaTensor(diagTensor);
+		//base->setCMassLocalPose(PxTransform(com, orient));
+
+
+		//create a joint if necessary
+		if (hasParentJoint)
+		{
+			btTransform offsetInA, offsetInB;
+			offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
+			offsetInB = localInertialFrame.inverse();
+			btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
+
+			bool disableParentCollision = true;
+
+			if (createMultiBody && cache.m_articulation)
+			{
+#if 0
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
+#endif
+			}
+		}
+
+
+		if (createMultiBody)
+		{
+
+		}
+		else
+		{
+			
+		}
+	}//was if 'shape/compountShape'
+
+	btAlignedObjectArray<int> urdfChildIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
+
+	int numChildren = urdfChildIndices.size();
+
+	if (recursive)
+	{
+		for (int i = 0; i < numChildren; i++)
+		{
+			int urdfChildLinkIndex = urdfChildIndices[i];
+
+			ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
+		}
+	}
+	return linkTransformInWorldSpace;
+}
+
+
+
+
+
+
+
+physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO)
+{
+	URDF2PhysXCachedData cache;
+	InitURDF2BulletCache(u2p, cache, flags);
+	int urdfLinkIndex = u2p.getRootLinkIndex();
+	int rootIndex = u2p.getRootLinkIndex();
+	B3_PROFILE("ConvertURDF2Bullet");
+
+	UrdfVisualShapeCache2 cachedLinkGraphicsShapesOut;
+	UrdfVisualShapeCache2 cachedLinkGraphicsShapes;
+
+	ArticulationCreationInterface creation;
+	creation.m_foundation = foundation;
+	creation.m_physics = physics;
+	creation.m_cooking = cooking;
+	creation.m_scene = scene;
+	creation.m_fileIO = fileIO;
+
+	bool createMultiBody = true;
+
+	bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0;
+	
+	if (recursive)
+	{
+		ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+
+	}
+	else
+	{
+#if 0
+		btAlignedObjectArray<btTransform> parentTransforms;
+		if (urdfLinkIndex >= parentTransforms.size())
+		{
+			parentTransforms.resize(urdfLinkIndex + 1);
+		}
+		parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
+		btAlignedObjectArray<childParentIndex> allIndices;
+
+		GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
+		allIndices.quickSort(MyIntCompareFunc);
+
+		for (int i = 0; i < allIndices.size(); i++)
+		{
+			int urdfLinkIndex = allIndices[i].m_index;
+			int parentIndex = allIndices[i].m_parentIndex;
+			btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
+			btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+			if ((urdfLinkIndex + 1) >= parentTransforms.size())
+			{
+				parentTransforms.resize(urdfLinkIndex + 1);
+			}
+			parentTransforms[urdfLinkIndex] = tr;
+		}
+#endif
+
+	}
+#if 0
+	if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
+	{
+		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
+	}
+#endif
+
+	
+	if (scene && cache.m_articulation)
+	{
+#ifdef DEBUG_ARTICULATIONS
+		printf("\n-----------------\n");
+		printf("m_linkTransWorldSpace\n");
+		for (int i = 0; i < cache.m_linkTransWorldSpace.size(); i++)
+		{
+			printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
+				cache.m_linkTransWorldSpace[i].p.x, cache.m_linkTransWorldSpace[i].p.y, cache.m_linkTransWorldSpace[i].p.z,
+				cache.m_linkTransWorldSpace[i].q.x, cache.m_linkTransWorldSpace[i].q.y, cache.m_linkTransWorldSpace[i].q.z, cache.m_linkTransWorldSpace[i].q.w);
+		}
+		printf("m_parentLocalPoses\n");
+		for (int i = 0; i < cache.m_parentLocalPoses.size(); i++)
+		{
+			printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
+				cache.m_parentLocalPoses[i].p.x, cache.m_parentLocalPoses[i].p.y, cache.m_parentLocalPoses[i].p.z,
+				cache.m_parentLocalPoses[i].q.x, cache.m_parentLocalPoses[i].q.y, cache.m_parentLocalPoses[i].q.z, cache.m_parentLocalPoses[i].q.w);
+		}
+
+		printf("m_childLocalPoses\n");
+		for (int i = 0; i < cache.m_childLocalPoses.size(); i++)
+		{
+			printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
+				cache.m_childLocalPoses[i].p.x, cache.m_childLocalPoses[i].p.y, cache.m_childLocalPoses[i].p.z,
+				cache.m_childLocalPoses[i].q.x, cache.m_childLocalPoses[i].q.y, cache.m_childLocalPoses[i].q.z, cache.m_childLocalPoses[i].q.w);
+		}
+
+		printf("m_geomDimensions\n");
+		for (int i = 0; i < cache.m_geomDimensions.size(); i++)
+		{
+			printf("PxVec3(%f,%f,%f),\n",
+				cache.m_geomDimensions[i].x, cache.m_geomDimensions[i].y, cache.m_geomDimensions[i].z);
+		}
+
+		printf("m_geomLocalPoses\n");
+		for (int i = 0; i < cache.m_geomLocalPoses.size(); i++)
+		{
+			printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
+				cache.m_geomLocalPoses[i].p.x, cache.m_geomLocalPoses[i].p.y, cache.m_geomLocalPoses[i].p.z,
+				cache.m_geomLocalPoses[i].q.x, cache.m_geomLocalPoses[i].q.y, cache.m_geomLocalPoses[i].q.z, cache.m_geomLocalPoses[i].q.w);
+		}
+
+		printf("m_linkMaterials\n");
+		for (int i = 0; i < cache.m_linkMaterials.size(); i++)
+		{
+			printf("PxVec3(%f,%f,%f),\n",
+				cache.m_linkMaterials[i].x, cache.m_linkMaterials[i].y, cache.m_linkMaterials[i].z);
+		}
+#endif //DEBUG_ARTICULATIONS
+
+		//see also https://github.com/NVIDIAGameWorks/PhysX/issues/43
+		if ((flags & CUF_USE_SELF_COLLISION) == 0)
+		{
+			physx::PxU32 nbActors = cache.m_articulation->getNbLinks();;     // Max number of actors expected in the aggregate
+			bool selfCollisions = false;
+			physx::PxAggregate* aggregate = physics->createAggregate(nbActors, selfCollisions);
+			aggregate->addArticulation(*cache.m_articulation);
+			scene->addAggregate(*aggregate);
+		}
+		else
+		{
+			scene->addArticulation(*cache.m_articulation);
+		}
+
+	}
+
+	return  cache.m_articulation;
+}

examples/SharedMemory/physx/URDF2PhysX.h

@@ -0,0 +1,25 @@
+#ifndef URDF2PHYSX_H
+#define URDF2PHYSX_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Importers/ImportURDFDemo/URDFJointTypes.h"
+
+namespace physx
+{
+	class PxFoundation;
+	class PxPhysics;
+	class PxDefaultCpuDispatcher;
+	class PxScene;
+	class PxCooking;
+	class PxArticulationReducedCoordinate;
+};
+
+struct UrdfVisualShapeCache2
+{
+	b3AlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
+	b3AlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
+};
+
+physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO);
+
+#endif //URDF2PHYSX_H

examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp

@@ -73,9 +73,9 @@ struct MyTexture3
 struct EGLRendererObjectArray
 {
 	btAlignedObjectArray<TinyRenderObjectData*> m_renderObjects;
+	btAlignedObjectArray<int> m_graphicsInstanceIds;
 	int m_objectUniqueId;
 	int m_linkIndex;
-	int m_graphicsInstanceId;
 	btTransform m_worldTransform;
 	btVector3 m_localScaling;
 
@@ -83,12 +83,15 @@ struct EGLRendererObjectArray
 	{
 		m_worldTransform.setIdentity();
 		m_localScaling.setValue(1, 1, 1);
-		m_graphicsInstanceId = -1;
 	}
 };
 
-#define START_WIDTH 2560
+//#define START_WIDTH 2560
-#define START_HEIGHT 2048
+//#define START_HEIGHT 2048
+
+#define START_WIDTH 1024
+#define START_HEIGHT 768
+
 
 struct EGLRendererVisualShapeConverterInternalData
 {
@@ -105,6 +108,7 @@ struct EGLRendererVisualShapeConverterInternalData
 
 	btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
 
+	
 	int m_upAxis;
 	int m_swWidth;
 	int m_swHeight;
@@ -132,12 +136,22 @@ struct EGLRendererVisualShapeConverterInternalData
 	int m_flags;
 	SimpleCamera m_camera;
 
+	bool m_leftMouseButton;
+	bool m_middleMouseButton;
+	bool m_rightMouseButton;
+	float m_wheelMultiplier;
+	float m_mouseMoveMultiplier;
+	float m_mouseXpos;
+	float m_mouseYpos;
+	bool m_mouseInitialized;
+	int m_graphicsUniqueIdGenerator;
+
 	EGLRendererVisualShapeConverterInternalData()
 		: m_upAxis(2),
 		  m_swWidth(START_WIDTH),
 		  m_swHeight(START_HEIGHT),
 		  m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB),
-		  m_lightDirection(btVector3(-5, 200, -40)),
+		  m_lightDirection(btVector3(-5, -40, 200 )),
 		  m_hasLightDirection(false),
 		  m_lightColor(btVector3(1.0, 1.0, 1.0)),
 		  m_hasLightColor(false),
@@ -150,7 +164,16 @@ struct EGLRendererVisualShapeConverterInternalData
 		  m_lightSpecularCoeff(0.05),
 		  m_hasLightSpecularCoeff(false),
 		  m_hasShadow(false),
-		  m_flags(0)
+		  m_flags(0),
+		m_leftMouseButton(false),
+		m_middleMouseButton(false),
+		m_rightMouseButton(false),
+		m_wheelMultiplier(0.01f),
+		m_mouseMoveMultiplier(0.4f),
+		m_mouseXpos(0.f),
+		m_mouseYpos(0.f),
+		m_mouseInitialized(false),
+		m_graphicsUniqueIdGenerator(15)
 	{
 		m_depthBuffer.resize(m_swWidth * m_swHeight);
 		m_shadowBuffer.resize(m_swWidth * m_swHeight);
@@ -161,7 +184,7 @@ struct EGLRendererVisualShapeConverterInternalData
 		m_window = new DefaultOpenGLWindow();
 		m_window->setAllowRetina(allowRetina);
 		b3gWindowConstructionInfo ci;
-		ci.m_title = "Title";
+		ci.m_title = "PyBullet";
 		ci.m_width = m_swWidth;
 		ci.m_height = m_swHeight;
 		ci.m_renderDevice = 0;
@@ -207,21 +230,234 @@ struct EGLRendererVisualShapeConverterInternalData
 	}
 };
 
+
+
+static EGLRendererVisualShapeConverter* gWindow = 0;
+
+static void SimpleResizeCallback(float widthf, float heightf)
+{
+	int width = (int)widthf;
+	int height = (int)heightf;
+	if (gWindow && gWindow->m_data->m_instancingRenderer)
+	{
+		gWindow->m_data->m_instancingRenderer->resize(width, height);
+	}
+
+	//if (gApp && gApp->m_instancingRenderer)
+	//	gApp->m_instancingRenderer->resize(width, height);
+	//
+	//if (gApp && gApp->m_primRenderer)
+	//	gApp->m_primRenderer->setScreenSize(width, height);
+}
+
+static void SimpleKeyboardCallback(int key, int state)
+{
+	if (key == B3G_ESCAPE) //&& gApp && gApp->m_window)
+	{
+		//gApp->m_window->setRequestExit();
+	}
+	else
+	{
+		//gApp->defaultKeyboardCallback(key,state);
+	}
+}
+
+static void SimpleMouseButtonCallback(int button, int state, float x, float y)
+{
+	if (gWindow)
+	{
+		gWindow->mouseButtonCallback(button, state, x, y);
+	}
+}
+
+static void SimpleMouseMoveCallback(float x, float y)
+{
+	if (gWindow)
+	{
+		gWindow->mouseMoveCallback(x, y);
+	}
+}
+
+static void SimpleWheelCallback(float deltax, float deltay)
+{
+	float wheelMultiplier = 0.01f;
+	if (gWindow && gWindow->m_data->m_instancingRenderer)
+	{
+		class GLInstancingRenderer* renderer = gWindow->m_data->m_instancingRenderer;
+		b3Vector3 cameraTargetPosition, cameraPosition, cameraUp = b3MakeVector3(0, 0, 0);
+		int upAxis = renderer->getActiveCamera()->getCameraUpAxis();
+		cameraUp[upAxis] = 1;
+		CommonCameraInterface* camera = renderer->getActiveCamera();
+
+		camera->getCameraPosition(cameraPosition);
+		camera->getCameraTargetPosition(cameraTargetPosition);
+
+		bool m_leftMouseButton = false;
+
+		if (!m_leftMouseButton)
+		{
+			float cameraDistance = camera->getCameraDistance();
+			if (deltay < 0 || cameraDistance > 1)
+			{
+				cameraDistance -= deltay * 0.01f;
+				if (cameraDistance < 1)
+					cameraDistance = 1;
+				camera->setCameraDistance(cameraDistance);
+			}
+			else
+			{
+				b3Vector3 fwd = cameraTargetPosition - cameraPosition;
+				fwd.normalize();
+				cameraTargetPosition += fwd * deltay * wheelMultiplier;  //todo: expose it in the GUI?
+			}
+		}
+		else
+		{
+			if (b3Fabs(deltax) > b3Fabs(deltay))
+			{
+				b3Vector3 fwd = cameraTargetPosition - cameraPosition;
+				b3Vector3 side = cameraUp.cross(fwd);
+				side.normalize();
+				cameraTargetPosition += side * deltax * wheelMultiplier;
+			}
+			else
+			{
+				cameraTargetPosition -= cameraUp * deltay * wheelMultiplier;
+			}
+		}
+
+		camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
+	}
+}
+
+void defaultMouseButtonCallback(int button, int state, float x, float y)
+{
+	if (gWindow)
+	{
+		gWindow->mouseButtonCallback(button, state, x, y);
+	}
+}
+void defaultMouseMoveCallback(float x, float y)
+{
+	if (gWindow)
+	{
+		gWindow->mouseMoveCallback(x, y);
+	}  //m_window &&  m_renderer
+
+	
+}
+
+void EGLRendererVisualShapeConverter::mouseButtonCallback(int button, int state, float x, float y)
+{
+	if (button == 0)
+		m_data->m_leftMouseButton = (state == 1);
+	if (button == 1)
+		m_data->m_middleMouseButton = (state == 1);
+
+	if (button == 2)
+		m_data->m_rightMouseButton = (state == 1);
+
+	m_data->m_mouseXpos = x;
+	m_data->m_mouseYpos = y;
+	m_data->m_mouseInitialized = true;
+}
+void EGLRendererVisualShapeConverter::mouseMoveCallback(float x, float y)
+{
+	class GLInstancingRenderer* renderer = m_data->m_instancingRenderer;
+	if (renderer == 0)
+		return;
+	
+	CommonCameraInterface* camera = renderer->getActiveCamera();
+
+	bool isAltPressed = m_data->m_window->isModifierKeyPressed(B3G_ALT);
+	bool isControlPressed = m_data->m_window->isModifierKeyPressed(B3G_CONTROL);
+
+	if (isAltPressed || isControlPressed)
+	{
+		float xDelta = x - m_data->m_mouseXpos;
+		float yDelta = y - m_data->m_mouseYpos;
+		float cameraDistance = camera->getCameraDistance();
+		float pitch = camera->getCameraPitch();
+		float yaw = camera->getCameraYaw();
+
+		float targPos[3];
+		float camPos[3];
+
+		camera->getCameraTargetPosition(targPos);
+		camera->getCameraPosition(camPos);
+
+		b3Vector3 cameraPosition = b3MakeVector3(b3Scalar(camPos[0]),
+			b3Scalar(camPos[1]),
+			b3Scalar(camPos[2]));
+
+		b3Vector3 cameraTargetPosition = b3MakeVector3(b3Scalar(targPos[0]),
+			b3Scalar(targPos[1]),
+			b3Scalar(targPos[2]));
+		b3Vector3 cameraUp = b3MakeVector3(0, 0, 0);
+		cameraUp[camera->getCameraUpAxis()] = 1.f;
+
+		if (m_data->m_leftMouseButton)
+		{
+			//			if (b3Fabs(xDelta)>b3Fabs(yDelta))
+			//			{
+			pitch -= yDelta * m_data->m_mouseMoveMultiplier;
+			//			} else
+			//			{
+			yaw -= xDelta * m_data->m_mouseMoveMultiplier;
+			//			}
+		}
+
+		if (m_data->m_middleMouseButton)
+		{
+			cameraTargetPosition += cameraUp * yDelta * 0.01;
+
+			b3Vector3 fwd = cameraTargetPosition - cameraPosition;
+			b3Vector3 side = cameraUp.cross(fwd);
+			side.normalize();
+			cameraTargetPosition += side * xDelta * 0.01;
+		}
+		if (m_data->m_rightMouseButton)
+		{
+			cameraDistance -= xDelta * 0.01f;
+			cameraDistance -= yDelta * 0.01f;
+			if (cameraDistance < 1)
+				cameraDistance = 1;
+			if (cameraDistance > 1000)
+				cameraDistance = 1000;
+		}
+		camera->setCameraDistance(cameraDistance);
+		camera->setCameraPitch(pitch);
+		camera->setCameraYaw(yaw);
+		camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
+	}
+	m_data->m_mouseXpos = x;
+	m_data->m_mouseYpos = y;
+	m_data->m_mouseInitialized = true;
+}
 EGLRendererVisualShapeConverter::EGLRendererVisualShapeConverter()
 {
+	
 	m_data = new EGLRendererVisualShapeConverterInternalData();
-
+	
+	
 	float dist = 1.5;
 	float pitch = -10;
 	float yaw = -80;
 	float targetPos[3] = {0, 0, 0};
 	m_data->m_camera.setCameraUpAxis(m_data->m_upAxis);
 	resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	gWindow = this;
+	m_data->m_window->setResizeCallback(SimpleResizeCallback);
+	m_data->m_window->setWheelCallback(SimpleWheelCallback);
+	m_data->m_window->setMouseButtonCallback(SimpleMouseButtonCallback);
+	m_data->m_window->setMouseMoveCallback(SimpleMouseMoveCallback);
 }
 EGLRendererVisualShapeConverter::~EGLRendererVisualShapeConverter()
 {
+	gWindow = 0;
 	resetAll();
 	delete m_data;
+	
 }
 
 void EGLRendererVisualShapeConverter::setLightDirection(float x, float y, float z)
@@ -649,11 +885,15 @@ static btVector4 sColors[4] =
 // If you are getting segfaults in this function it may be ecause you are
 // compliling the plugin with differently from pybullet, try complining the
 // plugin with distutils too.
-void EGLRendererVisualShapeConverter::convertVisualShapes(
+int EGLRendererVisualShapeConverter::convertVisualShapes(
 	int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
 	const UrdfLink* linkPtr, const UrdfModel* model,
-	int collisionObjectUniqueId, int bodyUniqueId, struct  CommonFileIOInterface* fileIO)
+	int orgGraphicsUniqueId, int bodyUniqueId, struct  CommonFileIOInterface* fileIO)
 {
+	if (orgGraphicsUniqueId< 0)
+	{
+		orgGraphicsUniqueId  = m_data->m_graphicsUniqueIdGenerator++;
+	}
 	btAssert(linkPtr);  // TODO: remove if (not doing it now, because diff will be 50+ lines)
 	if (linkPtr)
 	{
@@ -743,12 +983,12 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
 				}
 			}
 
-			EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
+			EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId];
 			if (visualsPtr == 0)
 			{
-				m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new EGLRendererObjectArray);
+				m_data->m_swRenderInstances.insert(orgGraphicsUniqueId, new EGLRendererObjectArray);
 			}
-			visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
+			visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId];
 
 			btAssert(visualsPtr);
 			EGLRendererObjectArray* visuals = *visualsPtr;
@@ -808,13 +1048,14 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
 
 					int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
 					double scaling[3] = {1, 1, 1};
-					visuals->m_graphicsInstanceId = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
+					int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
 
 					int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24);
 					{
-						int graphicsIndex = visuals->m_graphicsInstanceId;
 						if (graphicsIndex >= 0)
 						{
+							visuals->m_graphicsInstanceIds.push_back(graphicsIndex);
+
 							if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
 							{
 								m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
@@ -835,6 +1076,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
 			}
 		}
 	}
+	return orgGraphicsUniqueId;
 }
 
 int EGLRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
@@ -1032,7 +1274,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 	int numRemainingPixels = numTotalPixels - startPixelIndex;
 	int numBytesPerPixel = 4;  //RGBA
 	int numRequestedPixels = btMin(rgbaBufferSizeInPixels, numRemainingPixels);
-	if (numRequestedPixels)
+	if (1)
 	{
 		if (startPixelIndex == 0)
 		{
@@ -1049,9 +1291,13 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 			m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis);
 
 			m_data->m_instancingRenderer->renderScene();
+			m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3);
+			m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3);
+			m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3);
 
 			int numBytesPerPixel = 4;  //RGBA
 
+			if (pixelsRGBA || depthBuffer)
 			{
 				{
 					BT_PROFILE("copy pixels");
@@ -1083,40 +1329,40 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 						}
 					}
 				}
-			}
+			
-
+				m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel);
-			m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel);
+				m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr));
-			m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr));
+				//rescale and flip
-			//rescale and flip
-			{
-				BT_PROFILE("resize and flip");
-				for (int j = 0; j < *heightPtr; j++)
 				{
-					for (int i = 0; i < *widthPtr; i++)
+					BT_PROFILE("resize and flip");
+					for (int j = 0; j < *heightPtr; j++)
 					{
-						int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr)));
+						for (int i = 0; i < *widthPtr; i++)
-						int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr)));
-						btClamp(xIndex, 0, sourceWidth);
-						btClamp(yIndex, 0, sourceHeight);
-						int bytesPerPixel = 4;  //RGBA
-
-						int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
-						int sourceDepthIndex = xIndex + yIndex * sourceWidth;
-#define COPY4PIXELS 1
-#ifdef COPY4PIXELS
-						int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0];
-						int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0];
-						*dst = *src;
-
-#else
-						m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0];
-						m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1];
-						m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2];
-						m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255;
-#endif
-						if (depthBuffer)
 						{
-							m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex];
+							int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr)));
+							int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr)));
+							btClamp(xIndex, 0, sourceWidth);
+							btClamp(yIndex, 0, sourceHeight);
+							int bytesPerPixel = 4;  //RGBA
+
+							int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
+							int sourceDepthIndex = xIndex + yIndex * sourceWidth;
+	#define COPY4PIXELS 1
+	#ifdef COPY4PIXELS
+							int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0];
+							int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0];
+							*dst = *src;
+
+	#else
+							m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0];
+							m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1];
+							m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2];
+							m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255;
+	#endif
+							if (depthBuffer)
+							{
+								m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex];
+							}
 						}
 					}
 				}
@@ -1153,36 +1399,35 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
 						}
 					}
 				}
-			}
+				m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1);
 
-			m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1);
+				//rescale and flip
-
-			//rescale and flip
-			{
-				BT_PROFILE("resize and flip");
-				for (int j = 0; j < destinationHeight; j++)
 				{
-					for (int i = 0; i < destinationWidth; i++)
+					BT_PROFILE("resize and flip");
+					for (int j = 0; j < destinationHeight; j++)
 					{
-						int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth)));
+						for (int i = 0; i < destinationWidth; i++)
-						int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight)));
-						btClamp(xIndex, 0, sourceWidth);
-						btClamp(yIndex, 0, sourceHeight);
-						int bytesPerPixel = 4;  //RGBA
-						int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
-						int sourceDepthIndex = xIndex + yIndex * sourceWidth;
-
-						if (segmentationMaskBuffer)
 						{
-							float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex];
+							int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth)));
-							if (depth < 1)
+							int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight)));
+							btClamp(xIndex, 0, sourceWidth);
+							btClamp(yIndex, 0, sourceHeight);
+							int bytesPerPixel = 4;  //RGBA
+							int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
+							int sourceDepthIndex = xIndex + yIndex * sourceWidth;
+
+							if (segmentationMaskBuffer)
 							{
-								int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]);
+								float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex];
-								m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask;
+								if (depth < 1)
-							}
+								{
-							else
+									int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]);
-							{
+									m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask;
-								m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1;
+								}
+								else
+								{
+									m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1;
+								}
 							}
 						}
 					}
@@ -1257,7 +1502,11 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
 		{
 			for (int o = 0; o < ptr->m_renderObjects.size(); o++)
 			{
-				m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceId);
+				for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++)
+				{
+					m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]);
+				}
+				
 				delete ptr->m_renderObjects[o];
 			}
 		}
@@ -1387,11 +1636,15 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId,
 		EGLRendererObjectArray* renderObj = *renderObjPtr;
 		renderObj->m_worldTransform = worldTransform;
 		renderObj->m_localScaling = localScaling;
-		if (renderObj->m_graphicsInstanceId >= 0)
+		for (int i = 0; i < renderObj->m_graphicsInstanceIds.size(); i++)
 		{
-			btVector3 pos = worldTransform.getOrigin();
+			int graphicsInstanceId = renderObj->m_graphicsInstanceIds[i];
-			btQuaternion orn = worldTransform.getRotation();
+			if (graphicsInstanceId >= 0)
-			m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, renderObj->m_graphicsInstanceId);
+			{
+				btVector3 pos = worldTransform.getOrigin();
+				btQuaternion orn = worldTransform.getRotation();
+				m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, graphicsInstanceId);
+			}
 		}
 	}
 }

examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h

@@ -11,7 +11,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
 
 	virtual ~EGLRendererVisualShapeConverter();
 
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
 
 	virtual int getNumVisualShapes(int bodyUniqueId);
 
@@ -55,6 +55,10 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
 	virtual void setProjectiveTexture(bool useProjectiveTexture);
 
 	virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
+
+	virtual void mouseMoveCallback(float x, float y);
+	virtual void mouseButtonCallback(int button, int state, float x, float y);
+
 };
 
 #endif  //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp

@@ -0,0 +1,391 @@
+#include "btBulletDynamicsCommon.h"
+
+//for inverse dynamics, DeepMimic implementation
+#include "RBDModel.h"
+#include "RBDUtil.h"
+#include "KinTree.h"
+
+//for BulletInverseDynamics
+//#include "BulletInverseDynamics/IDConfig.hpp"
+//#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
+
+//#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+//#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+//#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
+
+
+struct TempLink
+{
+	int m_parentIndex;
+	const btCollisionObject* m_collider;
+	double m_mass;
+	int m_jointType;
+	int m_dofOffset;
+	int m_dofCount;
+	btVector3 m_dVector;
+	btVector3 m_eVector;
+	btQuaternion m_zeroRotParentToThis;
+	btQuaternion m_this_to_body1;
+};
+
+bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
+{
+
+	bool result = true;
+
+	int num_joints = links.size();
+
+	btAlignedObjectArray<btVector3> bodyToLinkPositions;
+	btAlignedObjectArray<btQuaternion> bodyToLinkRotations;
+	btAlignedObjectArray<btQuaternion> dVectorRot;
+	bodyToLinkRotations.resize(num_joints);
+	bodyToLinkPositions.resize(num_joints);
+	dVectorRot.resize(num_joints);
+
+	jointMat.resize(num_joints, 19);
+	bodyDefs.resize(num_joints, 17);
+	for (int i = 0; i < num_joints * 19; i++)
+	{
+		jointMat(i) = SIMD_INFINITY;
+	}
+	for (int i = 0; i < num_joints * 17; i++)
+	{
+		bodyDefs(i) = SIMD_INFINITY;
+	}
+
+	for (int i = 0; i < num_joints * 17; i++)
+	{
+		bodyDefs(i) = SIMD_INFINITY;
+	}
+
+	btScalar unk = -12345;
+
+	int totalDofs = 0;
+
+	for (int j = 0; j < num_joints; ++j)
+	{
+
+		int i = j;
+		int parentIndex = links[j].m_parentIndex;
+
+		cShape::eShape shapeType = cShape::eShapeNull;
+		double param0 = 0, param1 = 0, param2 = 0;
+		if (links[j].m_collider)
+		{
+			const btCollisionShape* collisionShape = links[j].m_collider->getCollisionShape();
+			if (collisionShape->isCompound())
+			{
+				const btCompoundShape* compound = (const btCompoundShape*)collisionShape;
+				if (compound->getNumChildShapes() > 0)
+				{
+					collisionShape = compound->getChildShape(0);
+				}
+			}
+			switch (collisionShape->getShapeType())
+			{
+			case BOX_SHAPE_PROXYTYPE:
+			{
+				shapeType = cShape::eShapeBox;
+				btBoxShape* box = (btBoxShape*)collisionShape;
+				param0 = box->getHalfExtentsWithMargin()[0] * 2;
+				param1 = box->getHalfExtentsWithMargin()[1] * 2;
+				param2 = box->getHalfExtentsWithMargin()[2] * 2;
+				break;
+			}
+			case SPHERE_SHAPE_PROXYTYPE:
+			{
+				btSphereShape* sphere = (btSphereShape*)collisionShape;
+				param0 = sphere->getRadius() * 2;
+				param1 = sphere->getRadius() * 2;
+				param2 = sphere->getRadius() * 2;
+				shapeType = cShape::eShapeSphere;
+				break;
+			}
+			case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				btCapsuleShape* caps = (btCapsuleShape*)collisionShape;
+				param0 = caps->getRadius() * 2;
+				param1 = caps->getHalfHeight() * 2;
+				param2 = caps->getRadius() * 2;
+				shapeType = cShape::eShapeCapsule;
+				break;
+			}
+			default:
+			{
+				btAssert(0);
+			}
+			}
+		}
+
+
+		btVector3 body_attach_pt1 = links[j].m_dVector;
+
+		//tQuaternion body_to_parent_body = parent_to_parent_body * this_to_parent * body_to_this;
+		//tQuaternion parent_to_parent_body = parent_body_to_parent.inverse();
+
+
+		bodyDefs(i, cKinTree::eBodyParam0) = param0;
+		bodyDefs(i, cKinTree::eBodyParam1) = param1;
+		bodyDefs(i, cKinTree::eBodyParam2) = param2;
+
+		bodyDefs(i, cKinTree::eBodyParamShape) = shapeType;
+		bodyDefs(i, cKinTree::eBodyParamMass) = links[j].m_mass;
+
+
+		bodyDefs(i, cKinTree::eBodyParamColGroup) = unk;
+		bodyDefs(i, cKinTree::eBodyParamEnableFallContact) = unk;
+
+		bodyDefs(i, cKinTree::eBodyColorR) = unk;
+		bodyDefs(i, cKinTree::eBodyColorG) = unk;
+		bodyDefs(i, cKinTree::eBodyColorB) = unk;
+		bodyDefs(i, cKinTree::eBodyColorA) = unk;
+
+		dVectorRot[j] = links[j].m_this_to_body1;
+
+		btVector3 body_attach_pt2 = quatRotate(links[j].m_this_to_body1.inverse(), body_attach_pt1);
+		bodyToLinkPositions[i] = body_attach_pt2;
+		bodyDefs(i, cKinTree::eBodyParamAttachX) = body_attach_pt2[0];
+		bodyDefs(i, cKinTree::eBodyParamAttachY) = body_attach_pt2[1];
+		bodyDefs(i, cKinTree::eBodyParamAttachZ) = body_attach_pt2[2];
+		btScalar bodyAttachThetaX = 0;
+		btScalar bodyAttachThetaY = 0;
+		btScalar bodyAttachThetaZ = 0;
+		btQuaternion body_to_this1 = links[j].m_this_to_body1.inverse();
+
+
+
+		body_to_this1.getEulerZYX(bodyAttachThetaZ, bodyAttachThetaY, bodyAttachThetaX);
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaX) = bodyAttachThetaX;
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaY) = bodyAttachThetaY;
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaZ) = bodyAttachThetaZ;
+
+		jointMat(i, cKinTree::eJointDescType) = links[j].m_jointType;
+		jointMat(i, cKinTree::eJointDescParent) = parentIndex;
+
+
+		btVector3 jointAttachPointMy = links[j].m_eVector;
+		btVector3 jointAttachPointMyv0 = jointAttachPointMy;
+		btVector3 parentBodyAttachPtMy(0, 0, 0);
+		btQuaternion parentBodyToLink;
+		parentBodyToLink = btQuaternion::getIdentity();
+		btQuaternion linkToParentBody = btQuaternion::getIdentity();
+		int parent_joint = links[j].m_parentIndex;
+
+		if (parent_joint != gInvalidIdx)
+		{
+			parentBodyAttachPtMy = bodyToLinkPositions[parent_joint];
+			parentBodyToLink = bodyToLinkRotations[parent_joint];
+			linkToParentBody = parentBodyToLink.inverse();
+		}
+		parentBodyAttachPtMy = quatRotate(linkToParentBody, parentBodyAttachPtMy);
+		//bodyToLinkRotations
+		jointAttachPointMy += parentBodyAttachPtMy;
+		jointAttachPointMy = quatRotate(linkToParentBody.inverse(), jointAttachPointMy);
+
+		btVector3 parent_body_attach_pt1(0, 0, 0);
+		if (parentIndex >= 0)
+		{
+			parent_body_attach_pt1 = links[parentIndex].m_dVector;
+		}
+		btQuaternion myparent_body_to_body(0, 0, 0, 1);
+		btQuaternion mybody_to_parent_body(0, 0, 0, 1);
+		btQuaternion parent_body_to_body1 = links[i].m_zeroRotParentToThis;
+		btQuaternion body_to_parent_body1 = parent_body_to_body1.inverse();
+
+		bodyToLinkRotations[i] = body_to_this1;
+
+		jointMat(i, cKinTree::eJointDescAttachX) = jointAttachPointMy[0];
+		jointMat(i, cKinTree::eJointDescAttachY) = jointAttachPointMy[1];
+		jointMat(i, cKinTree::eJointDescAttachZ) = jointAttachPointMy[2];
+
+
+		btQuaternion parent2parent_body2(0, 0, 0, 1);
+
+		if (parent_joint >= 0)
+		{
+			//parent2parent_body2 = bulletMB->getLink(parent_joint).m_dVectorRot;
+			parent2parent_body2 = dVectorRot[parent_joint];
+		}
+		///btQuaternion this2bodyA = bulletMB->getLink(j).m_dVectorRot;
+		btQuaternion this2bodyA = dVectorRot[j];
+
+		btQuaternion parent_body_2_body = links[j].m_zeroRotParentToThis;
+		btQuaternion combined2 = parent_body_2_body.inverse();
+		btQuaternion recoverthis2parent = parent2parent_body2.inverse()*combined2*this2bodyA;// body2this.inverse();
+		btScalar eulZ, eulY, eulX;
+		recoverthis2parent.getEulerZYX(eulZ, eulY, eulX);
+
+
+		jointMat(i, cKinTree::eJointDescAttachThetaX) = eulX;
+		jointMat(i, cKinTree::eJointDescAttachThetaY) = eulY;
+		jointMat(i, cKinTree::eJointDescAttachThetaZ) = eulZ;
+
+
+		jointMat(i, cKinTree::eJointDescLimLow0) = unk;
+		jointMat(i, cKinTree::eJointDescLimLow1) = unk;
+		jointMat(i, cKinTree::eJointDescLimLow2) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh0) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh1) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh2) = unk;
+		jointMat(i, cKinTree::eJointDescTorqueLim) = unk;
+
+		jointMat(i, cKinTree::eJointDescForceLim) = unk;
+		jointMat(i, cKinTree::eJointDescIsEndEffector) = unk;
+		jointMat(i, cKinTree::eJointDescDiffWeight) = unk;
+		jointMat(i, cKinTree::eJointDescParamOffset) = totalDofs;
+		totalDofs += links[j].m_dofCount;
+
+	}
+	return result;
+}
+
+
+void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
+{
+	btAlignedObjectArray<TempLink> links;
+
+	int numBaseShapes = 0;
+	if (bulletMB->getBaseCollider())
+	{
+		switch (bulletMB->getBaseCollider()->getCollisionShape()->getShapeType())
+		{
+			case CAPSULE_SHAPE_PROXYTYPE:
+			case SPHERE_SHAPE_PROXYTYPE:
+			case BOX_SHAPE_PROXYTYPE:
+			{
+				numBaseShapes++;
+				break;
+			}
+			case COMPOUND_SHAPE_PROXYTYPE:
+			{
+				btCompoundShape* compound = (btCompoundShape*)bulletMB->getBaseCollider()->getCollisionShape();
+				numBaseShapes += compound->getNumChildShapes();
+				break;
+			}
+			default:
+			{
+			}
+		}
+
+	}
+
+	//links include the 'base' and its childlinks
+	int baseLink = numBaseShapes? 1 : 0;
+	links.resize(bulletMB->getNumLinks() + baseLink);
+	for (int i = 0; i < links.size(); i++)
+	{
+		memset(&links[i], 0xffffffff, sizeof(TempLink));
+	}
+
+	int totalDofs = 0;
+	if (numBaseShapes)
+	{
+		//links[0] is the root/base
+		links[0].m_parentIndex = -1;
+		links[0].m_collider = bulletMB->getBaseCollider();
+		links[0].m_mass = bulletMB->getBaseMass();
+		links[0].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
+		links[0].m_dofOffset = 0;
+		links[0].m_dofCount = 7;
+		links[0].m_dVector.setValue(0, 0, 0);
+		links[0].m_eVector.setValue(0, 0, 0);
+		links[0].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
+		links[0].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
+		totalDofs = 7;
+	}
+	
+
+	for (int j = 0; j < bulletMB->getNumLinks(); ++j)
+	{
+		int parentIndex = bulletMB->getLink(j).m_parent;
+		links[j + baseLink].m_parentIndex = parentIndex + baseLink;
+		links[j + baseLink].m_collider = bulletMB->getLinkCollider(j);
+		links[j + baseLink].m_mass = bulletMB->getLink(j).m_mass;
+
+		int jointType = 0;
+		btQuaternion this_to_body1(0, 0, 0, 1);
+		int dofCount = 0;
+
+		if ((baseLink)==0 &&j == 0)//for 'root' either use fixed or none
+		{
+			dofCount = 7;
+			links[j].m_parentIndex = -1;
+			links[j].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
+			links[j].m_dofOffset = 0;
+			links[j].m_dofCount = dofCount;
+			
+			
+			links[j].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
+			//links[j].m_dVector.setValue(0, 0, 0);
+			links[j].m_dVector = bulletMB->getLink(j).m_dVector;
+			links[j].m_eVector.setValue(0, 0, 0);
+			//links[j].m_eVector = bulletMB->getLink(j).m_eVector;
+			links[j].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
+
+			links[j].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
+			totalDofs = 7;
+		}
+		else
+		{
+
+			switch (bulletMB->getLink(j).m_jointType)
+			{
+			case btMultibodyLink::eFixed:
+			{
+				jointType = cKinTree::eJointTypeFixed;
+				break;
+			}
+			case btMultibodyLink::ePrismatic:
+			{
+				dofCount = 1;
+				btVector3 refAxis(1, 0, 0);
+				btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
+				this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+				jointType = cKinTree::eJointTypePrismatic;
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			{
+				dofCount = 4;//??
+				jointType = cKinTree::eJointTypeSpherical;
+				break;
+			}
+			case btMultibodyLink::eRevolute:
+			{
+				dofCount = 1;
+				btVector3 refAxis(0, 0, 1);
+				btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
+				this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+				jointType = cKinTree::eJointTypeRevolute;
+				break;
+			}
+			default:
+			{
+			}
+			}
+			links[j + baseLink].m_jointType = jointType;
+			links[j + baseLink].m_dofOffset = totalDofs;
+			links[j + baseLink].m_dofCount = dofCount;
+			links[j + baseLink].m_dVector = bulletMB->getLink(j).m_dVector;
+			links[j + baseLink].m_eVector = bulletMB->getLink(j).m_eVector;
+			links[j + baseLink].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
+			links[j + baseLink].m_this_to_body1 = this_to_body1;
+		}
+
+		totalDofs += dofCount;
+	}
+
+	btExtractJointBodyFromTempLinks(links, bodyDefs, jointMat);
+}

examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h

@@ -0,0 +1,9 @@
+#ifndef BULLET_CONVERSION_H
+#define BULLET_CONVERSION_H
+
+class btMultiBody;
+#include "MathUtil.h"
+void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat);
+
+
+#endif  //BULLET_CONVERSION_H

examples/SharedMemory/plugins/stablePDPlugin/KinTree.h

@@ -0,0 +1,271 @@
+#pragma once
+
+#include <vector>
+#include <fstream>
+#include "Shape.h"
+#ifdef USE_JSON
+#include "json/json.h"
+#endif
+#include "MathUtil.h"
+
+class cKinTree
+{
+public:
+	// description of the joint tree representing an articulated figure
+	enum eJointType
+	{
+		eJointTypeRevolute,
+		eJointTypePlanar,
+		eJointTypePrismatic,
+		eJointTypeFixed,
+		eJointTypeSpherical,
+		eJointTypeNone,
+		eJointTypeMax
+	};
+
+	enum eJointDesc
+	{
+		eJointDescType,
+		eJointDescParent,
+		eJointDescAttachX,
+		eJointDescAttachY,
+		eJointDescAttachZ,
+		eJointDescAttachThetaX, // euler angles order rot(Z) * rot(Y) * rot(X)
+		eJointDescAttachThetaY,
+		eJointDescAttachThetaZ,
+		eJointDescLimLow0,
+		eJointDescLimLow1,
+		eJointDescLimLow2,
+		eJointDescLimHigh0,
+		eJointDescLimHigh1,
+		eJointDescLimHigh2,
+		eJointDescTorqueLim,
+		eJointDescForceLim,
+		eJointDescIsEndEffector,
+		eJointDescDiffWeight,
+		eJointDescParamOffset,
+		eJointDescMax
+	};
+	typedef Eigen::Matrix<double, 1, eJointDescMax> tJointDesc;
+
+	enum eBodyParam
+	{
+		eBodyParamShape,
+		eBodyParamMass,
+		eBodyParamColGroup, // 0 collides with nothing and 1 collides with everything
+		eBodyParamEnableFallContact,
+		eBodyParamAttachX,
+		eBodyParamAttachY,
+		eBodyParamAttachZ,
+		eBodyParamAttachThetaX, // Euler angles order XYZ
+		eBodyParamAttachThetaY,
+		eBodyParamAttachThetaZ,
+		eBodyParam0,
+		eBodyParam1,
+		eBodyParam2,
+		eBodyColorR,
+		eBodyColorG,
+		eBodyColorB,
+		eBodyColorA,
+		eBodyParamMax
+	};
+	typedef Eigen::Matrix<double, 1, eBodyParamMax> tBodyDef;
+
+	enum eDrawShape
+	{
+		eDrawShapeShape,
+		eDrawShapeParentJoint,
+		eDrawShapeAttachX,
+		eDrawShapeAttachY,
+		eDrawShapeAttachZ,
+		eDrawShapeAttachThetaX, // Euler angles order XYZ
+		eDrawShapeAttachThetaY,
+		eDrawShapeAttachThetaZ,
+		eDrawShapeParam0,
+		eDrawShapeParam1,
+		eDrawShapeParam2,
+		eDrawShapeColorR,
+		eDrawShapeColorG,
+		eDrawShapeColorB,
+		eDrawShapeColorA,
+		eDrawShapeMeshID,
+		eDrawShapeParamMax
+	};
+	typedef Eigen::Matrix<double, 1, eDrawShapeParamMax> tDrawShapeDef;
+
+	static const int gInvalidJointID;
+	static const int gPosDim;
+	static const int gRotDim;
+	static const int gRootDim;
+
+	static bool HasValidRoot(const Eigen::MatrixXd& joint_mat);
+	static tVector GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
+	static void SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state);
+	static tQuaternion GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
+	static void SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state);
+	static tVector GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
+	static void SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel);
+	static tVector GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
+	static void SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel);
+
+	static tVector CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tVector LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt);
+	static tQuaternion CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static void CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id, tVector& out_axis, double& out_theta);
+	
+	static tVector CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static tVector CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
+	static tVector CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static tVector CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
+
+	static int GetNumDof(const Eigen::MatrixXd& joint_mat);
+	static void ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose);
+
+	static Eigen::VectorXi FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end);
+	static bool IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len);
+	static double CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain);
+
+	static void CalcAABB(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max);
+
+	static int GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetJointParamSize(eJointType joint_type);
+	static void GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params);
+	static void SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state);
+	static eJointType GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetParent(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool HasParent(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id);
+	
+	static tVector GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id);
+	
+	static double CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id);
+
+	// calculates the longest chain in the subtree of each joint
+	static void CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths);
+	static void CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses);
+	
+	static tMatrix BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tMatrix ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+
+#ifdef USE_JSON
+	static bool Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat);
+	static bool ParseBodyDef(const Json::Value& root, tBodyDef& out_def);
+	static bool ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def);
+	static std::string BuildJointMatJson(const Eigen::MatrixXd& joint_mat);
+	static std::string BuildJointJson(int id, const tJointDesc& joint_desc);
+	static bool ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc);
+#endif
+
+	static int GetNumJoints(const Eigen::MatrixXd& joint_mat);
+	static int GetRoot(const Eigen::MatrixXd& joint_mat);
+	static void FindChildren(const Eigen::MatrixXd& joint_mat, int joint_id, Eigen::VectorXi& out_children);
+
+	static bool LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs);
+	
+	static bool LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs);
+	
+	static cShape::eShape GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id);
+	static void GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta);
+	static double GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id);
+	static int GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id);
+	static bool GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id);
+	static void SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs);
+	static tVector GetBodySize(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id);
+	static double CalcTotalMass(const Eigen::MatrixXd& body_defs);
+	static bool IsValidBody(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id);
+
+	static int GetDrawShapeParentJoint(const tDrawShapeDef& shape);
+	static tVector GetDrawShapeAttachPt(const tDrawShapeDef& shape);
+	static tVector GetDrawShapeAttachTheta(const tDrawShapeDef& shape);
+	static void GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta);
+	static tVector GetDrawShapeColor(const tDrawShapeDef& shape);
+	static int GetDrawShapeMeshID(const tDrawShapeDef& shape);
+
+	static tVector CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
+	static tVector CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id);
+	static void CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta);
+	static tMatrix BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
+	static tMatrix BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id);
+
+	static tJointDesc BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt);
+	static tJointDesc BuildJointDesc();
+	static tBodyDef BuildBodyDef();
+	static tDrawShapeDef BuildDrawShapeDef();
+
+	static void BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel);
+
+	static void CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
+	static tVector CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static tQuaternion CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+
+	static void CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
+	static tVector CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static tVector CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+	static double CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+	static double CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+
+	static void CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel);
+
+	static void PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose);
+	static void VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff);
+
+	static double CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tQuaternion CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tMatrix BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tMatrix BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel);
+
+	static void MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector<int> mirror_joints0, const std::vector<int> mirror_joints1, Eigen::VectorXd& out_pose);
+
+
+protected:
+	static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type);
+	static void PostProcessJointMat(Eigen::MatrixXd& out_joint_mat);
+
+	static tMatrix ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+
+	static void BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPosePlanar(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseFixed(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose);
+
+	static void BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelRevolute(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelPlanar(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelFixed(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelSpherical(Eigen::VectorXd& out_pose);
+
+	static void CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
+	static void CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
+};

examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Xue Bin Peng
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp

@@ -0,0 +1,904 @@
+#include "MathUtil.h"
+#include <time.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+cRand cMathUtil::gRand = cRand();
+
+int cMathUtil::Clamp(int val, int min, int max)
+{
+	return std::max(min, std::min(val, max));
+}
+
+void cMathUtil::Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec)
+{
+	out_vec = out_vec.cwiseMin(max).cwiseMax(min);
+}
+
+double cMathUtil::Clamp(double val, double min, double max)
+{
+	return std::max(min, std::min(val, max));
+}
+
+double cMathUtil::Saturate(double val)
+{
+	return Clamp(val, 0.0, 1.0);
+}
+
+double cMathUtil::Lerp(double t, double val0, double val1)
+{
+	return (1 - t) * val0 + t * val1;
+}
+
+double cMathUtil::NormalizeAngle(double theta)
+{
+	// normalizes theta to be between [-pi, pi]
+	double norm_theta = fmod(theta, 2 * M_PI);
+	if (norm_theta > M_PI)
+	{
+		norm_theta = -2 * M_PI + norm_theta;
+	}
+	else if (norm_theta < -M_PI)
+	{
+		norm_theta = 2 * M_PI + norm_theta;
+	}
+	return norm_theta;
+}
+
+double cMathUtil::RandDouble()
+{
+	return RandDouble(0, 1);
+}
+
+double cMathUtil::RandDouble(double min, double max)
+{
+	return gRand.RandDouble(min, max);
+}
+
+double cMathUtil::RandDoubleNorm(double mean, double stdev)
+{
+	return gRand.RandDoubleNorm(mean, stdev);
+}
+
+double cMathUtil::RandDoubleExp(double lambda)
+{
+	return gRand.RandDoubleExp(lambda);
+}
+
+double cMathUtil::RandDoubleSeed(double seed)
+{
+	unsigned int int_seed = *reinterpret_cast<unsigned int*>(&seed);
+	std::default_random_engine rand_gen(int_seed);
+	std::uniform_real_distribution<double> dist;
+	return dist(rand_gen);
+}
+
+int cMathUtil::RandInt()
+{
+	return gRand.RandInt();
+}
+
+int cMathUtil::RandInt(int min, int max)
+{
+	return gRand.RandInt(min, max);
+}
+
+int cMathUtil::RandUint()
+{
+	return gRand.RandUint();
+}
+
+int cMathUtil::RandUint(unsigned int min, unsigned int max)
+{
+	return gRand.RandUint(min, max);
+}
+
+int cMathUtil::RandIntExclude(int min, int max, int exc)
+{
+	return gRand.RandIntExclude(min, max, exc);
+}
+
+void cMathUtil::SeedRand(unsigned long int seed)
+{
+	gRand.Seed(seed);
+	srand(gRand.RandInt());
+}
+
+int cMathUtil::RandSign()
+{
+	return gRand.RandSign();
+}
+
+double cMathUtil::SmoothStep(double t)
+{
+	double val = t * t * t * (t * (t * 6 - 15) + 10);
+	return val;
+}
+
+bool cMathUtil::FlipCoin(double p)
+{
+	return gRand.FlipCoin(p);
+}
+
+tMatrix cMathUtil::TranslateMat(const tVector& trans)
+{
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 3) = trans[0];
+	mat(1, 3) = trans[1];
+	mat(2, 3) = trans[2];
+	return mat;
+}
+
+tMatrix cMathUtil::ScaleMat(double scale)
+{
+	return ScaleMat(tVector::Ones() * scale);
+}
+
+tMatrix cMathUtil::ScaleMat(const tVector& scale)
+{
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 0) = scale[0];
+	mat(1, 1) = scale[1];
+	mat(2, 2) = scale[2];
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tVector& euler)
+{
+	double x = euler[0];
+	double y = euler[1];
+	double z = euler[2];
+
+	double x_s = std::sin(x);
+	double x_c = std::cos(x);
+	double y_s = std::sin(y);
+	double y_c = std::cos(y);
+	double z_s = std::sin(z);
+	double z_c = std::cos(z);
+
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 0) = y_c * z_c;
+	mat(1, 0) = y_c * z_s;
+	mat(2, 0) = -y_s;
+
+	mat(0, 1) = x_s * y_s * z_c - x_c * z_s;
+	mat(1, 1) = x_s * y_s * z_s + x_c * z_c;
+	mat(2, 1) = x_s * y_c;
+
+	mat(0, 2) = x_c * y_s * z_c + x_s * z_s;
+	mat(1, 2) = x_c * y_s * z_s - x_s * z_c;
+	mat(2, 2) = x_c * y_c;
+
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tVector& axis, double theta)
+{
+	assert(std::abs(axis.squaredNorm() - 1) < 0.0001);
+	
+	double c = std::cos(theta);
+	double s = std::sin(theta);
+	double x = axis[0];
+	double y = axis[1];
+	double z = axis[2];
+
+	tMatrix mat;
+	mat <<	c + x * x * (1 - c),		x * y * (1 - c) - z * s,	x * z * (1 - c) + y * s,	0,
+			y * x * (1 - c) + z * s,	c + y * y * (1 - c),		y * z * (1 - c) - x * s,	0,
+			z * x * (1 - c) - y * s,	z * y * (1 - c) + x * s,	c + z * z * (1 - c),		0,
+			0,							0,							0,							1;
+
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tQuaternion& q)
+{
+	tMatrix mat = tMatrix::Identity();
+
+	double sqw = q.w() * q.w();
+	double sqx = q.x()*  q.x();
+	double sqy = q.y() * q.y();
+	double sqz = q.z() * q.z();
+	double invs = 1 / (sqx + sqy + sqz + sqw);
+
+	mat(0, 0) = (sqx - sqy - sqz + sqw) * invs;
+	mat(1, 1) = (-sqx + sqy - sqz + sqw) * invs;
+	mat(2, 2) = (-sqx - sqy + sqz + sqw) * invs;
+
+	double tmp1 = q.x()*q.y();
+	double tmp2 = q.z()*q.w();
+	mat(1, 0) = 2.0 * (tmp1 + tmp2) * invs;
+	mat(0, 1) = 2.0 * (tmp1 - tmp2) * invs;
+
+	tmp1 = q.x()*q.z();
+	tmp2 = q.y()*q.w();
+	mat(2, 0) = 2.0 * (tmp1 - tmp2) * invs;
+	mat(0, 2) = 2.0 * (tmp1 + tmp2) * invs;
+
+	tmp1 = q.y()*q.z();
+	tmp2 = q.x()*q.w();
+	mat(2, 1) = 2.0 * (tmp1 + tmp2) * invs;
+	mat(1, 2) = 2.0 * (tmp1 - tmp2) * invs;
+
+	return mat;
+}
+
+tMatrix cMathUtil::CrossMat(const tVector& a)
+{
+	tMatrix m;
+	m << 0,		-a[2],	a[1],	0,
+		 a[2],	0,		-a[0],	0,
+		 -a[1],	a[0],	0,		0,
+		 0,		0,		0,		1;
+	return m;
+}
+
+tMatrix cMathUtil::InvRigidMat(const tMatrix& mat)
+{
+	tMatrix inv_mat = tMatrix::Zero();
+	inv_mat.block(0, 0, 3, 3) = mat.block(0, 0, 3, 3).transpose();
+	inv_mat.col(3) = -inv_mat * mat.col(3);
+	inv_mat(3, 3) = 1;
+	return inv_mat;
+}
+
+tVector cMathUtil::GetRigidTrans(const tMatrix& mat)
+{
+	return tVector(mat(0, 3), mat(1, 3), mat(2, 3), 0);
+}
+
+tVector cMathUtil::InvEuler(const tVector& euler)
+{
+	tMatrix inv_mat = cMathUtil::RotateMat(tVector(1, 0, 0, 0), -euler[0])
+					* cMathUtil::RotateMat(tVector(0, 1, 0, 0), -euler[1])
+					* cMathUtil::RotateMat(tVector(0, 0, 1, 0), -euler[2]);
+	tVector inv_euler = cMathUtil::RotMatToEuler(inv_mat);
+	return inv_euler;
+}
+
+void cMathUtil::RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta)
+{
+	double c = (mat(0, 0) + mat(1, 1) + mat(2, 2) - 1) * 0.5;
+	c = cMathUtil::Clamp(c, -1.0, 1.0);
+
+	out_theta = std::acos(c);
+	if (std::abs(out_theta) < 0.00001)
+	{
+		out_axis = tVector(0, 0, 1, 0);
+	}
+	else
+	{
+		double m21 = mat(2, 1) - mat(1, 2);
+		double m02 = mat(0, 2) - mat(2, 0);
+		double m10 = mat(1, 0) - mat(0, 1);
+		double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
+		out_axis[0] = m21 / denom;
+		out_axis[1] = m02 / denom;
+		out_axis[2] = m10 / denom;
+		out_axis[3] = 0;
+	}
+}
+
+tVector cMathUtil::RotMatToEuler(const tMatrix& mat)
+{
+	tVector euler;
+	euler[0] = std::atan2(mat(2, 1), mat(2, 2));
+	euler[1] = std::atan2(-mat(2, 0), std::sqrt(mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2)));
+	euler[2] = std::atan2(mat(1, 0), mat(0, 0)); 
+	euler[3] = 0;
+	return euler;
+}
+
+tQuaternion cMathUtil::RotMatToQuaternion(const tMatrix& mat)
+{
+	double tr = mat(0, 0) + mat(1, 1) + mat(2, 2);
+	tQuaternion q;
+
+	if (tr > 0) {
+		double S = sqrt(tr + 1.0) * 2; // S=4*qw 
+		q.w() = 0.25 * S;
+		q.x() = (mat(2, 1) - mat(1, 2)) / S;
+		q.y() = (mat(0, 2) - mat(2, 0)) / S;
+		q.z() = (mat(1, 0) - mat(0, 1)) / S;
+	}
+	else if ((mat(0, 0) > mat(1, 1) && (mat(0, 0) > mat(2, 2)))) {
+		double S = sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2)) * 2; // S=4*qx 
+		q.w() = (mat(2, 1) - mat(1, 2)) / S;
+		q.x() = 0.25 * S;
+		q.y() = (mat(0, 1) + mat(1, 0)) / S;
+		q.z() = (mat(0, 2) + mat(2, 0)) / S;
+	}
+	else if (mat(1, 1) > mat(2, 2)) {
+		double S = sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2)) * 2; // S=4*qy
+		q.w() = (mat(0, 2) - mat(2, 0)) / S;
+		q.x() = (mat(0, 1) + mat(1, 0)) / S;
+		q.y() = 0.25 * S;
+		q.z() = (mat(1, 2) + mat(2, 1)) / S;
+	}
+	else {
+		double S = sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1)) * 2; // S=4*qz
+		q.w() = (mat(1, 0) - mat(0, 1)) / S;
+		q.x() = (mat(0, 2) + mat(2, 0)) / S;
+		q.y() = (mat(1, 2) + mat(2, 1)) / S;
+		q.z() = 0.25 * S;
+	}
+
+	return q;
+}
+
+void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta)
+{
+	double x = euler[0];
+	double y = euler[1];
+	double z = euler[2];
+
+	double x_s = std::sin(x);
+	double x_c = std::cos(x);
+	double y_s = std::sin(y);
+	double y_c = std::cos(y);
+	double z_s = std::sin(z);
+	double z_c = std::cos(z);
+
+	double c = (y_c * z_c + x_s * y_s * z_s + x_c * z_c + x_c * y_c - 1) * 0.5;
+	c = Clamp(c, -1.0, 1.0);
+
+	out_theta = std::acos(c);
+	if (std::abs(out_theta) < 0.00001)
+	{
+		out_axis = tVector(0, 0, 1, 0);
+	}
+	else
+	{
+		double m21 = x_s * y_c - x_c * y_s * z_s + x_s * z_c;
+		double m02 = x_c * y_s * z_c + x_s * z_s + y_s;
+		double m10 = y_c * z_s - x_s * y_s * z_c + x_c * z_s;
+		double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
+		out_axis[0] = m21 / denom;
+		out_axis[1] = m02 / denom;
+		out_axis[2] = m10 / denom;
+		out_axis[3] = 0;
+	}
+}
+
+tVector cMathUtil::AxisAngleToEuler(const tVector& axis, double theta)
+{
+	tQuaternion q = AxisAngleToQuaternion(axis, theta);
+	return QuaternionToEuler(q);
+}
+
+tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up)
+{
+	tVector x = up.cross3(dir);
+	double x_norm = x.norm();
+	if (x_norm == 0)
+	{
+		x_norm = 1;
+		x = (dir.dot(up) >= 0) ? tVector(1, 0, 0, 0) : tVector(-1, 0, 0, 0);
+	}
+	x /= x_norm;
+
+	tVector y = dir.cross3(x).normalized();
+	tVector z = dir;
+
+	tMatrix mat = tMatrix::Identity();
+	mat.block(0, 0, 3, 1) = x.segment(0, 3);
+	mat.block(0, 1, 3, 1) = y.segment(0, 3);
+	mat.block(0, 2, 3, 1) = z.segment(0, 3);
+
+	return mat;
+}
+
+void cMathUtil::DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
+							tVector& out_axis, double& out_theta)
+{
+	tMatrix R0 = RotateMat(axis0, theta0);
+	tMatrix R1 = RotateMat(axis1, theta1);
+	tMatrix M = DeltaRot(R0, R1);
+	RotMatToAxisAngle(M, out_axis, out_theta);
+}
+
+tMatrix cMathUtil::DeltaRot(const tMatrix& R0, const tMatrix& R1)
+{
+	return R1 * R0.transpose();
+}
+
+tQuaternion cMathUtil::EulerToQuaternion(const tVector& euler)
+{
+	tVector axis;
+	double theta;
+	EulerToAxisAngle(euler, axis, theta);
+	return AxisAngleToQuaternion(axis, theta);
+}
+
+tVector cMathUtil::QuaternionToEuler(const tQuaternion& q)
+{
+	double sinr = 2.0 * (q.w() * q.x() + q.y() * q.z());
+	double cosr = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
+	double x = std::atan2(sinr, cosr);
+
+	double sinp = 2.0 * (q.w() * q.y() - q.z() * q.x());
+	double y = 0;
+	if (fabs(sinp) >= 1)
+	{
+		y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+	}
+	else
+	{
+		y = asin(sinp);
+	}
+
+	double siny = 2.0 * (q.w() * q.z() + q.x() * q.y());
+	double cosy = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
+	double z = std::atan2(siny, cosy);
+
+	return tVector(x, y, z, 0);
+}
+
+tQuaternion cMathUtil::AxisAngleToQuaternion(const tVector& axis, double theta)
+{
+	// axis must be normalized
+	double c = std::cos(theta / 2);
+	double s = std::sin(theta / 2);
+	tQuaternion q;
+	q.w() = c;
+	q.x() = s * axis[0];
+	q.y() = s * axis[1];
+	q.z() = s * axis[2];
+	return q;
+}
+
+void cMathUtil::QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta)
+{
+	out_theta = 0;
+	out_axis = tVector(0, 0, 1, 0);
+
+	tQuaternion q1 = q;
+	if (q1.w() > 1)
+	{
+		q1.normalize();
+	}
+
+	double sin_theta = std::sqrt(1 - q1.w() * q1.w());
+	if (sin_theta > 0.000001)
+	{
+		out_theta = 2 * std::acos(q1.w());
+		out_theta = cMathUtil::NormalizeAngle(out_theta);
+		out_axis = tVector(q1.x(), q1.y(), q1.z(), 0) / sin_theta;
+	}
+}
+
+tMatrix cMathUtil::BuildQuaternionDiffMat(const tQuaternion& q)
+{
+	tMatrix mat;
+	mat << -0.5 * q.x(), -0.5 * q.y(), -0.5 * q.z(), 0,
+		0.5 * q.w(), -0.5 * q.z(), 0.5 * q.y(), 0,
+		0.5 * q.z(), 0.5 * q.w(), -0.5 * q.x(), 0,
+		-0.5 * q.y(), 0.5 * q.x(), 0.5 * q.w(), 0;
+	return mat;
+}
+
+tVector cMathUtil::CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt)
+{
+	tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1);
+	tVector axis;
+	double theta;
+	QuaternionToAxisAngle(q_diff, axis, theta);
+	return (theta / dt) * axis;
+}
+
+tVector cMathUtil::CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt)
+{
+	// calculate relative rotational velocity in the coordinate frame of q0
+	tQuaternion q_diff = q0.conjugate() * q1;
+	tVector axis;
+	double theta;
+	QuaternionToAxisAngle(q_diff, axis, theta);
+	return (theta / dt) * axis;
+}
+
+tQuaternion cMathUtil::VecToQuat(const tVector& v)
+{
+	return tQuaternion(v[0], v[1], v[2], v[3]);
+}
+
+tVector cMathUtil::QuatToVec(const tQuaternion& q)
+{
+	return tVector(q.w(), q.x(), q.y(), q.z());
+}
+
+tQuaternion cMathUtil::QuatDiff(const tQuaternion& q0, const tQuaternion& q1)
+{
+	return q1 * q0.conjugate();
+}
+
+double cMathUtil::QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1)
+{
+	tQuaternion dq = QuatDiff(q0, q1);
+	return QuatTheta(dq);
+}
+
+double cMathUtil::QuatTheta(const tQuaternion& dq)
+{
+	double theta = 0;
+	tQuaternion q1 = dq;
+	if (q1.w() > 1)
+	{
+		q1.normalize();
+	}
+
+	double sin_theta = std::sqrt(1 - q1.w() * q1.w());
+	if (sin_theta > 0.0001)
+	{
+		theta = 2 * std::acos(q1.w());
+		theta = cMathUtil::NormalizeAngle(theta);
+	}
+	return theta;
+}
+
+tQuaternion cMathUtil::VecDiffQuat(const tVector& v0, const tVector& v1)
+{
+	return tQuaternion::FromTwoVectors(v0.segment(0, 3), v1.segment(0, 3));
+}
+
+tVector cMathUtil::QuatRotVec(const tQuaternion& q, const tVector& dir)
+{
+	tVector rot_dir = tVector::Zero();
+	rot_dir.segment(0, 3)  = q * dir.segment(0, 3);
+	return rot_dir;
+}
+
+tQuaternion cMathUtil::MirrorQuaternion(const tQuaternion& q, eAxis axis)
+{
+	tQuaternion mirror_q;
+	mirror_q.w() = q.w();
+	mirror_q.x() = (axis == eAxisX) ? q.x() : -q.x();
+	mirror_q.y() = (axis == eAxisY) ? q.y() : -q.y();
+	mirror_q.z() = (axis == eAxisZ) ? q.z() : -q.z();
+	return mirror_q;
+}
+
+double cMathUtil::Sign(double val)
+{
+	return SignAux<double>(val);
+}
+
+int cMathUtil::Sign(int val)
+{
+	return SignAux<int>(val);
+}
+
+double cMathUtil::AddAverage(double avg0, int count0, double avg1, int count1)
+{
+	double total = count0 + count1;
+	return (count0 / total) * avg0 + (count1 / total) * avg1;
+}
+
+tVector cMathUtil::AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1)
+{
+	double total = count0 + count1;
+	return (count0 / total) * avg0 + (count1 / total) * avg1 ;
+}
+
+void cMathUtil::AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result)
+{
+	double total = count0 + count1;
+	out_result = (count0 / total) * avg0 + (count1 / total) * avg1;
+}
+
+void cMathUtil::CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob)
+{
+	assert(out_prob.size() == vals.size());
+	int num_vals = static_cast<int>(vals.size());
+	double sum = 0;
+	double max_val = vals.maxCoeff();
+	for (int i = 0; i < num_vals; ++i)
+	{
+		double val = vals[i];
+		val = std::exp((val - max_val) / temp);
+		out_prob[i] = val;
+		sum += val;
+	}
+
+	out_prob /= sum;
+}
+
+double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
+{
+	assert(mean.size() == covar.size());
+	assert(sample.size() == covar.size());
+
+	Eigen::VectorXd diff = sample - mean;
+	double exp_val = diff.dot(diff.cwiseQuotient(covar));
+	double likelihood = std::exp(-0.5 * exp_val);
+
+	double partition = CalcGaussianPartition(covar);
+	likelihood /= partition;
+	return likelihood;
+}
+
+double cMathUtil::EvalGaussian(double mean, double covar, double sample)
+{
+	double diff = sample - mean;
+	double exp_val = diff * diff / covar;
+	double norm = 1 / std::sqrt(2 * M_PI * covar);
+	double likelihood = norm * std::exp(-0.5 * exp_val);
+	return likelihood;
+}
+
+double cMathUtil::CalcGaussianPartition(const Eigen::VectorXd& covar)
+{
+	int data_size = static_cast<int>(covar.size());
+	double det = covar.prod();
+	double partition = std::sqrt(std::pow(2 * M_PI, data_size) * det);
+	return partition;
+}
+
+double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
+{
+	int data_size = static_cast<int>(covar.size());
+
+	Eigen::VectorXd diff = sample - mean;
+	double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
+	double det = covar.prod();
+	logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));
+
+	return logp;
+}
+
+double cMathUtil::EvalGaussianLogp(double mean, double covar, double sample)
+{
+	double diff = sample - mean;
+	double logp = -0.5 * diff * diff / covar;
+	logp += -0.5 * (std::log(2 * M_PI) + std::log(covar));
+	return logp;
+}
+
+double cMathUtil::Sigmoid(double x)
+{
+	return Sigmoid(x, 1, 0);
+}
+
+double cMathUtil::Sigmoid(double x, double gamma, double bias)
+{
+	double exp = -gamma * (x + bias);
+	double val = 1 / (1 + std::exp(exp));
+	return val;
+}
+
+int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs)
+{
+	assert(std::abs(probs.sum() - 1) < 0.00001);
+	double rand = RandDouble();
+
+	int rand_idx = gInvalidIdx;
+	int num_probs = static_cast<int>(probs.size());
+	for (int i = 0; i < num_probs; ++i)
+	{
+		double curr_prob = probs[i];
+		rand -= curr_prob;
+
+		if (rand <= 0)
+		{
+			rand_idx = i;
+			break;
+		}
+	}
+	return rand_idx;
+}
+
+tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c)
+{
+	tVector v0 = b - a;
+	tVector v1 = c - a;
+	tVector v2 = p - a;
+
+	double d00 = v0.dot(v0);
+	double d01 = v0.dot(v1);
+	double d11 = v1.dot(v1);
+	double d20 = v2.dot(v0);
+	double d21 = v2.dot(v1);
+	double denom = d00 * d11 - d01 * d01;
+	double v = (d11 * d20 - d01 * d21) / denom;
+	double w = (d00 * d21 - d01 * d20) / denom;
+	double u = 1.0f - v - w;
+
+	return tVector(u, v, w, 0);
+}
+
+bool cMathUtil::ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
+{
+	bool contains = pt[0] >= aabb_min[0] && pt[1] >= aabb_min[1] && pt[2] >= aabb_min[2]
+					&& pt[0] <= aabb_max[0] && pt[1] <= aabb_max[1] && pt[2] <= aabb_max[2];
+	return contains;
+}
+
+bool cMathUtil::ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	return ContainsAABB(aabb_min0, aabb_min1, aabb_max1) && ContainsAABB(aabb_max0, aabb_min1, aabb_max1);
+}
+
+bool cMathUtil::ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
+{
+	bool contains = pt[0] >= aabb_min[0] && pt[2] >= aabb_min[2]
+		&& pt[0] <= aabb_max[0] && pt[2] <= aabb_max[2];
+	return contains;
+}
+
+bool cMathUtil::ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	return ContainsAABBXZ(aabb_min0, aabb_min1, aabb_max1) && ContainsAABBXZ(aabb_max0, aabb_min1, aabb_max1);
+}
+
+void cMathUtil::CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+									tVector& out_min, tVector& out_max)
+{
+	out_min = aabb_min0.cwiseMax(aabb_min1);
+	out_max = aabb_max0.cwiseMin(aabb_max1);
+	if (out_min[0] > out_max[0])
+	{
+		out_min[0] = 0;
+		out_max[0] = 0;
+	}
+	if (out_min[1] > out_max[1])
+	{
+		out_min[1] = 0;
+		out_max[1] = 0;
+	}
+	if (out_min[2] > out_max[2])
+	{
+		out_min[2] = 0;
+		out_max[2] = 0;
+	}
+}
+
+void cMathUtil::CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+								tVector& out_min, tVector& out_max)
+{
+	out_min = aabb_min0.cwiseMin(aabb_min1);
+	out_max = aabb_max0.cwiseMax(aabb_max1);
+}
+
+bool cMathUtil::IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
+	tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
+	tVector size0 = aabb_max0 - aabb_min0;
+	tVector size1 = aabb_max1 - aabb_min1;
+	tVector test_len = 0.5 * (size0 + size1);
+	tVector delta = center1 - center0;
+	bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[1]) <= test_len[1]) && (std::abs(delta[2]) <= test_len[2]);
+	return overlap;
+}
+
+bool cMathUtil::IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
+	tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
+	tVector size0 = aabb_max0 - aabb_min0;
+	tVector size1 = aabb_max1 - aabb_min1;
+	tVector test_len = 0.5 * (size0 + size1);
+	tVector delta = center1 - center0;
+	bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[2]) <= test_len[2]);
+	return overlap;
+}
+
+bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size)
+{
+	double pad = 0.001 * delta;
+	int curr_count = static_cast<int>(std::floor((curr_val + pad) / int_size));
+	int prev_count = static_cast<int>(std::floor((curr_val + pad - delta) / int_size));
+	bool new_action = (curr_count != prev_count);
+	return new_action;
+}
+
+tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max)
+{
+	tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]),
+						RandDouble(bound_min[1], bound_max[1]),
+						RandDouble(bound_min[2], bound_max[2]), 0);
+	return pt;
+}
+
+tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max)
+{
+	return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min));
+}
+
+tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus)
+{
+	double t = RandDouble(0, 1);
+	tVector size = bound_max - bound_min;
+	tVector new_min = focus + (t * 0.5) * size;
+	tVector new_max = focus - (t * 0.5) * size;
+	tVector offset = (bound_min - new_min).cwiseMax(0);
+	offset += (bound_max - new_max).cwiseMin(0);
+	new_min += offset;
+	new_max += offset;
+
+	return SampleRandPt(new_min, new_max);
+}
+
+void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist)
+{
+	assert(std::abs(dir.norm() - 1) < 0.000001);
+	assert(std::abs(q.norm() - 1) < 0.000001);
+
+	tVector q_axis = tVector(q.x(), q.y(), q.z(), 0);
+	double p = q_axis.dot(dir);
+	tVector twist_axis = p * dir;
+	out_twist = tQuaternion(q.w(), twist_axis[0], twist_axis[1], twist_axis[2]);
+	out_twist.normalize();
+	out_swing = q * out_twist.conjugate();
+}
+
+tQuaternion cMathUtil::ProjectQuat(const tQuaternion& q, const tVector& dir)
+{
+	assert(std::abs(dir.norm() - 1) < 0.00001);
+	tVector ref_axis = tVector::Zero();
+	int min_idx = 0;
+	dir.cwiseAbs().minCoeff(&min_idx);
+	ref_axis[min_idx] = 1;
+
+	tVector rot_dir0 = dir.cross3(ref_axis);
+	tVector rot_dir1 = cMathUtil::QuatRotVec(q, rot_dir0);
+	rot_dir1 -= rot_dir1.dot(dir) * dir;
+
+	double dir1_norm = rot_dir1.norm();
+	tQuaternion p_rot = tQuaternion::Identity();
+	if (dir1_norm > 0.0001)
+	{
+		rot_dir1 /= dir1_norm;
+		p_rot = cMathUtil::VecDiffQuat(rot_dir0, rot_dir1);
+	}
+	return p_rot;
+}
+
+void cMathUtil::ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x)
+{
+	double sampling_rate = 1 / dt;
+	int n = static_cast<int>(out_x.size());
+
+	double wc = std::tan(cutoff * M_PI / sampling_rate);
+	double k1 = std::sqrt(2) * wc;
+	double k2 = wc * wc;
+	double a = k2 / (1 + k1 + k2);
+	double b = 2 * a;
+	double c = a;
+	double k3 = b / k2;
+	double d = -2 * a + k3;
+	double e = 1 - (2 * a) - k3;
+
+	double xm2 = out_x[0];
+	double xm1 = out_x[0];
+	double ym2 = out_x[0];
+	double ym1 = out_x[0];
+
+	for (int s = 0; s < n; ++s) 
+	{
+		double x = out_x[s];
+		double y = a * x + b * xm1 + c * xm2 + d * ym1 + e * ym2;
+
+		out_x[s] = y;
+		xm2 = xm1;
+		xm1 = x;
+		ym2 = ym1;
+		ym1 = y;
+	}
+
+	double yp2 = out_x[n - 1];
+	double yp1 = out_x[n - 1];
+	double zp2 = out_x[n - 1];
+	double zp1 = out_x[n - 1];
+
+	for (int t = n - 1; t >= 0; --t) 
+	{
+		double y = out_x[t];
+		double z = a * y + b * yp1 + c * yp2 + d * zp1 + e * zp2;
+
+		out_x[t] = z;
+		yp2 = yp1;
+		yp1 = y;
+		zp2 = zp1;
+		zp1 = z;
+	}
+}

examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h

@@ -0,0 +1,155 @@
+#pragma once
+
+#include <random>
+
+#include "Eigen/Dense"
+#include "Eigen/StdVector"
+#include "Eigen/Geometry"
+#include "Rand.h"
+#define _USE_MATH_DEFINES
+#include "math.h"
+
+const int gInvalidIdx = -1;
+
+// for convenience define standard vector for rendering
+typedef Eigen::Vector4d tVector;
+typedef Eigen::Vector4d tVector3;
+typedef Eigen::Matrix4d tMatrix;
+typedef Eigen::Matrix3d tMatrix3;
+typedef Eigen::Quaterniond tQuaternion;
+
+template <typename T>
+using tEigenArr = std::vector<T, Eigen::aligned_allocator<T> >;
+typedef tEigenArr<tVector> tVectorArr;
+
+const double gRadiansToDegrees = 57.2957795;
+const double gDegreesToRadians = 1.0 / gRadiansToDegrees;
+const tVector gGravity = tVector(0, -9.8, 0, 0);
+const double gInchesToMeters = 0.0254;
+const double gFeetToMeters = 0.3048;
+
+class cMathUtil
+{
+public:
+	enum eAxis
+	{
+		eAxisX,
+		eAxisY,
+		eAxisZ,
+		eAxisMax
+	};
+
+	static int Clamp(int val, int min, int max);
+	static void Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec);
+	static double Clamp(double val, double min, double max);
+	static double Saturate(double val);
+	static double Lerp(double t, double val0, double val1);
+
+	static double NormalizeAngle(double theta);
+
+	// rand number
+	static double RandDouble();
+	static double RandDouble(double min, double max);
+	static double RandDoubleNorm(double mean, double stdev);
+	static double RandDoubleExp(double lambda);
+	static double RandDoubleSeed(double seed);
+	static int RandInt();
+	static int RandInt(int min, int max);
+	static int RandUint();
+	static int RandUint(unsigned int min, unsigned int max);
+	static int RandIntExclude(int min, int max, int exc);
+	static void SeedRand(unsigned long int seed);
+	static int RandSign();
+	static bool FlipCoin(double p = 0.5);
+	static double SmoothStep(double t);
+
+	// matrices
+	static tMatrix TranslateMat(const tVector& trans);
+	static tMatrix ScaleMat(double scale);
+	static tMatrix ScaleMat(const tVector& scale);
+	static tMatrix RotateMat(const tVector& euler); // euler angles order rot(Z) * rot(Y) * rot(X)
+	static tMatrix RotateMat(const tVector& axis, double theta);
+	static tMatrix RotateMat(const tQuaternion& q);
+	static tMatrix CrossMat(const tVector& a);
+	// inverts a transformation consisting only of rotations and translations
+	static tMatrix InvRigidMat(const tMatrix& mat);
+	static tVector GetRigidTrans(const tMatrix& mat);
+	static tVector InvEuler(const tVector& euler);
+	static void RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta);
+	static tVector RotMatToEuler(const tMatrix& mat);
+	static tQuaternion RotMatToQuaternion(const tMatrix& mat);
+	static void EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta);
+	static tVector AxisAngleToEuler(const tVector& axis, double theta);
+	static tMatrix DirToRotMat(const tVector& dir, const tVector& up);
+
+	static void DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
+							tVector& out_axis, double& out_theta);
+	static tMatrix DeltaRot(const tMatrix& R0, const tMatrix& R1);
+
+	static tQuaternion EulerToQuaternion(const tVector& euler);
+	static tVector QuaternionToEuler(const tQuaternion& q);
+	static tQuaternion AxisAngleToQuaternion(const tVector& axis, double theta);
+	static void QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta);
+	static tMatrix BuildQuaternionDiffMat(const tQuaternion& q);
+	static tVector CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt);
+	static tVector CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt);
+	static tQuaternion VecToQuat(const tVector& v);
+	static tVector QuatToVec(const tQuaternion& q);
+	static tQuaternion QuatDiff(const tQuaternion& q0, const tQuaternion& q1);
+	static double QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1);
+	static double QuatTheta(const tQuaternion& dq);
+	static tQuaternion VecDiffQuat(const tVector& v0, const tVector& v1);
+	static tVector QuatRotVec(const tQuaternion& q, const tVector& dir);
+	static tQuaternion MirrorQuaternion(const tQuaternion& q, eAxis axis);
+
+	static double Sign(double val);
+	static int Sign(int val);
+
+	static double AddAverage(double avg0, int count0, double avg1, int count1);
+	static tVector AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1);
+	static void AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result);
+	static void CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob);
+	static double EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
+	static double EvalGaussian(double mean, double covar, double sample);
+	static double CalcGaussianPartition(const Eigen::VectorXd& covar);
+	static double EvalGaussianLogp(double mean, double covar, double sample);
+	static double EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
+	static double Sigmoid(double x);
+	static double Sigmoid(double x, double gamma, double bias);
+
+	static int SampleDiscreteProb(const Eigen::VectorXd& probs);
+	static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c);
+
+	static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
+	static bool ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static bool ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
+	static bool ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static void CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+									tVector& out_min, tVector& out_max);
+	static void CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+								tVector& out_min, tVector& out_max);
+	static bool IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static bool IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+
+	// check if curr_val and curr_val - delta belong to different intervals
+	static bool CheckNextInterval(double delta, double curr_val, double int_size);
+
+	static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max);
+	// samples a bound within the given bounds with a benter towards the focus pt
+	static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max);
+	static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus);
+
+	static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist);
+	static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir);
+
+	static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x);
+	
+private:
+	static cRand gRand;
+
+	template <typename T>
+	static T SignAux(T val)
+	{
+		return (T(0) < val) - (val < T(0));
+	}
+};

examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp

@@ -0,0 +1,245 @@
+#include "RBDModel.h"
+#include "RBDUtil.h"
+#include "KinTree.h"
+
+cRBDModel::cRBDModel()
+{
+}
+
+cRBDModel::~cRBDModel()
+{
+}
+
+void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity)
+{
+	assert(joint_mat.rows() == body_defs.rows());
+	mGravity = gravity;
+	mJointMat = joint_mat;
+	mBodyDefs = body_defs;
+
+	int num_dofs = GetNumDof();
+	int num_joints = GetNumJoints();
+	const int svs = cSpAlg::gSpVecSize;
+
+	mPose = Eigen::VectorXd::Zero(num_dofs);
+	mVel = Eigen::VectorXd::Zero(num_dofs);
+
+	tMatrix trans_mat;
+	InitJointSubspaceArr();
+	mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols());
+	mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols);
+	mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
+	mBiasForce = Eigen::VectorXd::Zero(num_dofs);
+	mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs);
+}
+
+void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
+{
+	SetPose(pose);
+	SetVel(vel);
+
+	UpdateJointSubspaceArr();
+	UpdateChildParentMatArr();
+	UpdateSpWorldTrans();
+	UpdateMassMat();
+	UpdateBiasForce();
+}
+
+int cRBDModel::GetNumDof() const
+{
+	return cKinTree::GetNumDof(mJointMat);
+}
+
+int cRBDModel::GetNumJoints() const
+{
+	return cKinTree::GetNumJoints(mJointMat);
+}
+
+void cRBDModel::SetGravity(const tVector& gravity)
+{
+	mGravity = gravity;
+}
+
+
+
+const tVector& cRBDModel::GetGravity() const
+{
+	return mGravity;
+}
+
+const Eigen::MatrixXd& cRBDModel::GetJointMat() const
+{
+	return mJointMat;
+}
+
+const Eigen::MatrixXd& cRBDModel::GetBodyDefs() const
+{
+	return mBodyDefs;
+}
+
+const Eigen::VectorXd& cRBDModel::GetPose() const
+{
+	return mPose;
+}
+
+const Eigen::VectorXd& cRBDModel::GetVel() const
+{
+	return mVel;
+}
+
+int cRBDModel::GetParent(int j) const
+{
+	return cKinTree::GetParent(mJointMat, j);
+}
+
+const Eigen::MatrixXd& cRBDModel::GetMassMat() const
+{
+	return mMassMat;
+}
+
+const Eigen::VectorXd& cRBDModel::GetBiasForce() const
+{
+	return mBiasForce;
+}
+
+Eigen::MatrixXd& cRBDModel::GetInertiaBuffer()
+{
+	return mInertiaBuffer;
+}
+
+tMatrix cRBDModel::GetChildParentMat(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	tMatrix trans;
+	int r = static_cast<int>(trans.rows());
+	int c = static_cast<int>(trans.cols());
+	trans = mChildParentMatArr.block(j * r, 0, r, c);
+	return trans;
+}
+
+tMatrix cRBDModel::GetParentChildMat(int j) const
+{
+	tMatrix child_parent_trans = GetChildParentMat(j);
+	tMatrix parent_child_trans = cMathUtil::InvRigidMat(child_parent_trans);
+	return parent_child_trans;
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpChildParentTrans(int j) const
+{
+	tMatrix mat = GetChildParentMat(j);
+	return cSpAlg::MatToTrans(mat);
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpParentChildTrans(int j) const
+{
+	tMatrix mat = GetParentChildMat(j);
+	return cSpAlg::MatToTrans(mat);
+}
+
+tMatrix cRBDModel::GetWorldJointMat(int j) const
+{
+	cSpAlg::tSpTrans trans = GetSpWorldJointTrans(j);
+	return cSpAlg::TransToMat(trans);
+}
+
+tMatrix cRBDModel::GetJointWorldMat(int j) const
+{
+	cSpAlg::tSpTrans trans = GetSpJointWorldTrans(j);
+	return cSpAlg::TransToMat(trans);
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpWorldJointTrans(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	cSpAlg::tSpTrans trans = cSpAlg::GetTrans(mSpWorldJointTransArr, j);
+	return trans;
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpJointWorldTrans(int j) const
+{
+	cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
+	return cSpAlg::InvTrans(world_joint_trans);
+}
+
+const Eigen::Block<const Eigen::MatrixXd> cRBDModel::GetJointSubspace(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	int offset = cKinTree::GetParamOffset(mJointMat, j);
+	int dim = cKinTree::GetParamSize(mJointMat, j);
+	int r = static_cast<int>(mJointSubspaceArr.rows());
+	return mJointSubspaceArr.block(0, offset, r, dim);
+}
+
+tVector cRBDModel::CalcJointWorldPos(int j) const
+{
+	cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
+	tVector r = cSpAlg::GetRad(world_joint_trans);
+	return r;
+}
+
+void cRBDModel::SetPose(const Eigen::VectorXd& pose)
+{
+	mPose = pose;
+}
+
+void cRBDModel::SetVel(const Eigen::VectorXd& vel)
+{
+	mVel = vel;
+}
+
+void cRBDModel::InitJointSubspaceArr()
+{
+	int num_dofs = GetNumDof();
+	int num_joints = GetNumJoints();
+	mJointSubspaceArr = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs);
+	for (int j = 0; j < num_joints; ++j)
+	{
+		int offset = cKinTree::GetParamOffset(mJointMat, j);
+		int dim = cKinTree::GetParamSize(mJointMat, j);
+		int r = static_cast<int>(mJointSubspaceArr.rows());
+		mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
+	}
+}
+
+void cRBDModel::UpdateJointSubspaceArr()
+{
+	int num_joints = GetNumJoints();
+	for (int j = 0; j < num_joints; ++j)
+	{
+		bool const_subspace = cRBDUtil::IsConstJointSubspace(mJointMat, j);
+		if (!const_subspace)
+		{
+			int offset = cKinTree::GetParamOffset(mJointMat, j);
+			int dim = cKinTree::GetParamSize(mJointMat, j);
+			int r = static_cast<int>(mJointSubspaceArr.rows());
+			mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
+		}
+	}
+}
+
+void cRBDModel::UpdateChildParentMatArr()
+{
+	int num_joints = GetNumJoints();
+	for (int j = 0; j < num_joints; ++j)
+	{
+		tMatrix child_parent_trans = cKinTree::ChildParentTrans(mJointMat, mPose, j);
+		int r = static_cast<int>(child_parent_trans.rows());
+		int c = static_cast<int>(child_parent_trans.cols());
+		mChildParentMatArr.block(j * r, 0, r, c) = child_parent_trans;
+	}
+}
+
+void cRBDModel::UpdateSpWorldTrans()
+{
+	cRBDUtil::CalcWorldJointTransforms(*this, mSpWorldJointTransArr);
+}
+
+void cRBDModel::UpdateMassMat()
+{
+	cRBDUtil::BuildMassMat(*this, mInertiaBuffer, mMassMat);
+}
+
+void cRBDModel::UpdateBiasForce()
+{
+	cRBDUtil::BuildBiasForce(*this, mBiasForce);
+}

examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "SpAlg.h"
+#include "MathUtil.h"
+
+// this class is mostly to help with efficiency by precomputing some useful
+// quantities for RBD calculations
+class cRBDModel
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	cRBDModel();
+	~cRBDModel();
+
+	virtual void Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity);
+	virtual void Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
+
+	virtual int GetNumDof() const;
+	virtual int GetNumJoints() const;
+
+	virtual const tVector& GetGravity() const;
+	virtual void SetGravity(const tVector& gravity);
+
+	virtual const Eigen::MatrixXd& GetJointMat() const;
+	virtual const Eigen::MatrixXd& GetBodyDefs() const;
+	virtual const Eigen::VectorXd& GetPose() const;
+	virtual const Eigen::VectorXd& GetVel() const;
+	virtual int GetParent(int j) const;
+
+	virtual const Eigen::MatrixXd& GetMassMat() const;
+	virtual const Eigen::VectorXd& GetBiasForce() const;
+	virtual Eigen::MatrixXd& GetInertiaBuffer();
+
+	virtual tMatrix GetChildParentMat(int j) const;
+	virtual tMatrix GetParentChildMat(int j) const;
+	virtual cSpAlg::tSpTrans GetSpChildParentTrans(int j) const;
+	virtual cSpAlg::tSpTrans GetSpParentChildTrans(int j) const;
+
+	virtual tMatrix GetWorldJointMat(int j) const;
+	virtual tMatrix GetJointWorldMat(int j) const;
+	virtual cSpAlg::tSpTrans GetSpWorldJointTrans(int j) const;
+	virtual cSpAlg::tSpTrans GetSpJointWorldTrans(int j) const;
+
+	virtual const Eigen::Block<const Eigen::MatrixXd> GetJointSubspace(int j) const;
+	virtual tVector CalcJointWorldPos(int j) const;
+
+protected:
+	tVector mGravity;
+	Eigen::MatrixXd mJointMat;
+	Eigen::MatrixXd mBodyDefs;
+	Eigen::VectorXd mPose;
+	Eigen::VectorXd mVel;
+
+	Eigen::MatrixXd mJointSubspaceArr;
+	Eigen::MatrixXd mChildParentMatArr;
+	Eigen::MatrixXd mSpWorldJointTransArr;
+	Eigen::MatrixXd mMassMat;
+	Eigen::VectorXd mBiasForce;
+	Eigen::MatrixXd mInertiaBuffer;
+
+	virtual void SetPose(const Eigen::VectorXd& pose);
+	virtual void SetVel(const Eigen::VectorXd& vel);
+
+	virtual void InitJointSubspaceArr();
+
+	virtual void UpdateJointSubspaceArr();
+	virtual void UpdateChildParentMatArr();
+	virtual void UpdateSpWorldTrans();
+	virtual void UpdateMassMat();
+	virtual void UpdateBiasForce();
+};

examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <vector>
+#include <fstream>
+
+#include "RBDModel.h"
+#include "KinTree.h"
+
+class cRBDUtil
+{
+public:
+	static void SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, Eigen::VectorXd& out_tau);
+
+	static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, Eigen::VectorXd& out_acc);
+	static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, const Eigen::VectorXd& total_force, Eigen::VectorXd& out_acc);
+
+	static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat);
+	static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buffer, Eigen::MatrixXd& out_mass_mat);
+	static void BuildBiasForce(const cRBDModel& model, Eigen::VectorXd& out_bias_force);
+
+	static void CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_force);
+
+	static void BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Eigen::MatrixXd& out_J);
+	static void BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int joint_id, Eigen::MatrixXd& out_J);
+	static Eigen::MatrixXd MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel, const Eigen::MatrixXd& J, int joint_id);
+
+	static void BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
+	static Eigen::MatrixXd ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& J, int joint_id);
+
+	static void BuildCOMJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
+	static void BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J, Eigen::MatrixXd& out_J);
+	static void BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_dot);
+
+	static cSpAlg::tSpVec BuildCOMVelProdAcc(const cRBDModel& model);
+	static cSpAlg::tSpVec BuildCOMVelProdAccAux(const cRBDModel& model, const Eigen::MatrixXd& Jd);
+	static cSpAlg::tSpVec CalcVelProdAcc(const cRBDModel& model, const Eigen::MatrixXd& Jd, int joint_id);
+
+	static cSpAlg::tSpVec CalcJointWorldVel(const cRBDModel& model, int joint_id);
+	static cSpAlg::tSpVec CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static cSpAlg::tSpVec CalcWorldVel(const cRBDModel& model, int joint_id);
+	static cSpAlg::tSpVec CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+
+
+	static tVector CalcCoMPos(const cRBDModel& model);
+	static tVector CalcCoMVel(const cRBDModel& model);
+	static tVector CalcCoMVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
+	static void CalcCoM(const cRBDModel& model, tVector& out_com, tVector& out_vel);
+	static void CalcCoM(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel,
+						tVector& out_com, tVector& out_vel);
+
+	static cSpAlg::tSpTrans BuildParentChildTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static cSpAlg::tSpTrans BuildChildParentTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	
+	// extract a cSpAlg::tSpTrans from a matrix presentating a stack of transforms
+	static void CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd& out_trans_arr);
+	static bool IsConstJointSubspace(const Eigen::MatrixXd& joint_mat, int j);
+	static Eigen::MatrixXd BuildJointSubspace(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+
+	static cSpAlg::tSpMat BuildMomentInertia(const Eigen::MatrixXd& body_defs, int part_id);
+
+protected:
+
+	static Eigen::MatrixXd BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static Eigen::MatrixXd BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+
+	static cSpAlg::tSpVec BuildCj(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+
+	static cSpAlg::tSpMat BuildMomentInertiaBox(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaCapsule(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaSphere(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaCylinder(const Eigen::MatrixXd& body_defs, int part_id);
+
+	// builds the spatial inertial matrix in the coordinate frame of the parent joint
+	static cSpAlg::tSpMat BuildInertiaSpatialMat(const Eigen::MatrixXd& body_defs, int part_id);
+};

examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp

@@ -0,0 +1,140 @@
+#include "Rand.h"
+#include <time.h>
+#include <assert.h>
+#include <algorithm>
+
+cRand::cRand()
+{
+	unsigned long int seed = static_cast<unsigned long int>(time(NULL));
+	mRandGen = std::default_random_engine(seed);
+	mRandDoubleDist = std::uniform_real_distribution<double>(0, 1);
+	mRandDoubleDistNorm = std::normal_distribution<double>(0, 1);
+	mRandIntDist = std::uniform_int_distribution<int>(std::numeric_limits<int>::min() + 1, std::numeric_limits<int>::max()); // + 1 since there is one more neg int than pos int
+	mRandUintDist = std::uniform_int_distribution<unsigned int>(std::numeric_limits<unsigned int>::min(), std::numeric_limits<unsigned int>::max());
+}
+
+cRand::cRand(unsigned long int seed) 
+{
+	Seed(seed);
+}
+
+cRand::~cRand()
+{
+}
+
+double cRand::RandDouble()
+{
+	return mRandDoubleDist(mRandGen);
+}
+
+double cRand::RandDouble(double min, double max)
+{
+	if (min == max)
+	{
+		return min;
+	}
+
+	// generate random double in [min, max]
+	double rand_double = mRandDoubleDist(mRandGen);
+	rand_double = min + (rand_double * (max - min));
+	return rand_double;
+}
+
+double cRand::RandDoubleExp(double lambda)
+{
+	std::exponential_distribution<double> dist(lambda);
+	double rand_double = dist(mRandGen);
+	return rand_double;
+}
+
+double cRand::RandDoubleNorm(double mean, double stdev)
+{
+	double rand_double = mRandDoubleDistNorm(mRandGen);
+	rand_double = mean + stdev * rand_double;
+	return rand_double;
+}
+
+int cRand::RandInt()
+{
+	return mRandIntDist(mRandGen);
+}
+
+int cRand::RandInt(int min, int max)
+{
+	if (min == max)
+	{
+		return min;
+	}
+
+	// generate random double in [min, max)
+	int delta = max - min;
+	int rand_int = std::abs(RandInt());
+	rand_int = min + rand_int % delta;
+
+	return rand_int;
+}
+
+int cRand::RandUint()
+{
+	return mRandUintDist(mRandGen);
+}
+
+int cRand::RandUint(unsigned int min, unsigned int max)
+{
+	if (min == max)
+	{
+		return min;
+	}
+
+	// generate random double in [min, max)
+	int delta = max - min;
+	int rand_int = RandUint();
+	rand_int = min + rand_int % delta;
+
+	return rand_int;
+}
+
+int cRand::RandIntExclude(int min, int max, int exc)
+{
+	int rand_int = 0;
+	if (exc < min || exc >= max)
+	{
+		rand_int = RandInt(min, max);
+	}
+	else
+	{
+		int new_max = max - 1;
+		if (new_max <= min)
+		{
+			rand_int = min;
+		}
+		else
+		{
+			rand_int = RandInt(min, new_max);
+			if (rand_int >= exc)
+			{
+				++rand_int;
+			}
+		}
+	}
+	return rand_int;
+}
+
+void cRand::Seed(unsigned long int seed)
+{
+	mRandGen.seed(seed);
+	mRandDoubleDist.reset();
+	mRandDoubleDistNorm.reset();
+	mRandIntDist.reset();
+	mRandUintDist.reset();
+}
+
+int cRand::RandSign()
+{
+	return FlipCoin() ? -1 : 1;
+}
+
+bool cRand::FlipCoin(double p)
+{
+	return (RandDouble(0, 1) < p);
+}

examples/SharedMemory/plugins/stablePDPlugin/Rand.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <random>
+
+class cRand
+{
+public:
+	cRand();
+	cRand(unsigned long int seed);
+	virtual ~cRand();
+
+	virtual double RandDouble();
+	virtual double RandDouble(double min, double max);
+	virtual double RandDoubleExp(double lambda);
+	virtual double RandDoubleNorm(double mean, double stdev);
+	virtual int RandInt();
+	virtual int RandInt(int min, int max);
+	virtual int RandUint();
+	virtual int RandUint(unsigned int min, unsigned int max);
+	virtual int RandIntExclude(int min, int max, int exc);
+	virtual void Seed(unsigned long int seed);
+	virtual int RandSign();
+	virtual bool FlipCoin(double p = 0.5);
+
+private:
+	std::default_random_engine mRandGen;
+	std::uniform_real_distribution<double> mRandDoubleDist;
+	std::normal_distribution<double> mRandDoubleDistNorm;
+	std::uniform_int_distribution<int> mRandIntDist;
+	std::uniform_int_distribution<unsigned int> mRandUintDist;
+};

examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp

@@ -0,0 +1,37 @@
+#include "Shape.h"
+#include <assert.h>
+
+bool cShape::ParseShape(const std::string& str, eShape& out_shape)
+{
+	bool succ = true;
+	if (str == "null")
+	{
+		out_shape = eShapeNull;
+	}
+	else if (str == "box")
+	{
+		out_shape = eShapeBox;
+	}
+	else if (str == "capsule")
+	{
+		out_shape = eShapeCapsule;
+	}
+	else if (str == "sphere")
+	{
+		out_shape = eShapeSphere;
+	}
+	else if (str == "cylinder")
+	{
+		out_shape = eShapeCylinder;
+	}
+	else if (str == "plane")
+	{
+		out_shape = eShapePlane;
+	}
+	else
+	{
+		printf("Unsupported body shape %s\n", str.c_str());
+		assert(false);
+	}
+	return succ;
+}

examples/SharedMemory/plugins/stablePDPlugin/Shape.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <string>
+
+class cShape
+{
+public:
+	enum eShape
+	{
+		eShapeNull,
+		eShapeBox,
+		eShapeCapsule,
+		eShapeSphere,
+		eShapeCylinder,
+		eShapePlane,
+		eShapeMax,
+	};
+
+	static bool ParseShape(const std::string& str, cShape::eShape& out_shape);
+};

examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp

@@ -0,0 +1,356 @@
+#include "SpAlg.h"
+#include <iostream>
+
+const int cSpAlg::gSpVecSize;
+const int cSpAlg::gSVTransRows;
+const int cSpAlg::gSVTransCols;
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1)
+{
+	return ConvertCoordM(m0, origin0, origin1, tMatrix::Identity());
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0,
+												const tVector& origin1, const tMatrix& R1)
+{
+	tMatrix R = R1 * R0.transpose();
+	return ConvertCoordM(m0, origin0, origin1, R);
+}
+
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R)
+{
+	tSpTrans X = BuildTrans(R, origin1 - origin0);
+	return ApplyTransM(X, m0);
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tVector& origin1)
+{
+	return ConvertCoordF(f0, origin0, origin1, tMatrix::Identity());
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tMatrix& R0,
+	const tVector& origin1, const tMatrix& R1)
+{
+	tMatrix R = R1 * R0.transpose();
+	return ConvertCoordF(f0, origin0, origin1, R);
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0,
+	const tVector& origin1, const tMatrix& R)
+{
+	tSpTrans X = BuildTrans(R, origin1 - origin0);
+	return ApplyTransF(X, f0);
+}
+
+
+cSpAlg::tSpVec cSpAlg::CrossM(const tSpVec& sv, const tSpVec& m)
+{
+	tVector sv_o = GetOmega(sv);
+	tVector sv_v = GetV(sv);
+	tVector m_o = GetOmega(m);
+	tVector m_v = GetV(m);
+
+	tVector o = sv_o.cross3(m_o);
+	tVector v = sv_v.cross3(m_o) + sv_o.cross3(m_v);
+
+	return BuildSV(o, v);
+}
+
+Eigen::MatrixXd cSpAlg::CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms)
+{
+	assert(ms.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, ms.cols());
+	for (int i = 0; i < ms.cols(); ++i)
+	{
+		const tSpVec& curr_m = ms.col(i);
+		result.col(i) = CrossM(sv, curr_m);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::CrossF(const tSpVec& sv, const tSpVec& f)
+{
+	tVector sv_o = GetOmega(sv);
+	tVector sv_v = GetV(sv);
+	tVector f_o = GetOmega(f);
+	tVector f_v = GetV(f);
+
+	tVector o = sv_o.cross3(f_o) + sv_v.cross3(f_v);
+	tVector v = sv_o.cross3(f_v);
+
+	return BuildSV(o, v);
+}
+
+Eigen::MatrixXd cSpAlg::CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs)
+{
+	assert(fs.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, fs.cols());
+	for (int i = 0; i < fs.cols(); ++i)
+	{
+		const tSpVec& curr_f = fs.col(i);
+		result.col(i) = CrossF(sv, curr_f);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& v)
+{
+	return BuildSV(tVector::Zero(), v);
+}
+
+cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& o, const tVector& v)
+{
+	tSpVec sv;
+	SetOmega(o, sv);
+	SetV(v, sv);
+	return sv;
+}
+
+tVector cSpAlg::GetOmega(const tSpVec& sv)
+{
+	tVector o = tVector::Zero();
+	o.block(0, 0, 3, 1) = sv.block(0, 0, 3, 1);
+	return o;
+}
+
+void cSpAlg::SetOmega(const tVector& o, tSpVec& out_sv)
+{
+	out_sv.block(0, 0, 3, 1) = o.block(0, 0, 3, 1);
+}
+
+tVector cSpAlg::GetV(const tSpVec& sv)
+{
+	tVector v = tVector::Zero();
+	v.block(0, 0, 3, 1) = sv.block(3, 0, 3, 1);
+	return v;
+}
+
+void cSpAlg::SetV(const tVector& v, tSpVec& out_sv)
+{
+	out_sv.block(3, 0, 3, 1) = v.block(0, 0, 3, 1);
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans()
+{
+	return BuildTrans(tMatrix::Identity(), tVector::Zero());
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans(const tMatrix& E, const tVector& r)
+{
+	tSpTrans X;
+	SetRad(r, X);
+	SetRot(E, X);
+	return X;
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans(const tVector& r)
+{
+	return BuildTrans(tMatrix::Identity(), r);
+}
+
+cSpAlg::tSpTrans cSpAlg::MatToTrans(const tMatrix& mat)
+{
+	tMatrix E = mat;
+	tVector r = mat.col(3);
+	r[3] = 0;
+	r = -E.transpose() * r;
+	return BuildTrans(E, r);
+}
+
+tMatrix cSpAlg::TransToMat(const tSpTrans& X)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix m = E;
+	m.col(3) = -E * r;
+	m(3, 3) = 1;
+	return m;
+}
+
+cSpAlg::tSpMat cSpAlg::BuildSpatialMatM(const tSpTrans& X)
+{
+	tSpMat m = tSpMat::Zero();
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix Er = E * cMathUtil::CrossMat(r);
+
+	m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 0, 3, 3) = -Er.block(0, 0, 3, 3);
+	return m;
+}
+
+cSpAlg::tSpMat cSpAlg::BuildSpatialMatF(const tSpTrans& X)
+{
+	tSpMat m = tSpMat::Zero();
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix Er = E * cMathUtil::CrossMat(r);
+
+	m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(0, 3, 3, 3) = -Er.block(0, 0, 3, 3);
+	return m;
+}
+
+
+cSpAlg::tSpTrans cSpAlg::InvTrans(const tSpTrans& X)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tSpTrans inv_X = BuildTrans(E.transpose(), -E * r);
+	return inv_X;
+}
+
+tMatrix cSpAlg::GetRot(const tSpTrans& X)
+{
+	tMatrix E = tMatrix::Zero();
+	E.block(0, 0, 3, 3) = X.block(0, 0, 3, 3);
+	return E;
+}
+
+void cSpAlg::SetRot(const tMatrix& E, tSpTrans& out_X)
+{
+	out_X.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+}
+
+tVector cSpAlg::GetRad(const tSpTrans& X)
+{
+	tVector r = tVector::Zero();
+	r.block(0, 0, 3, 1) = X.block(0, 3, 3, 1);
+	return r;
+}
+
+void cSpAlg::SetRad(const tVector& r, tSpTrans& out_X)
+{
+	out_X.block(0, 3, 3, 1) = r.block(0, 0, 3, 1);
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyTransM(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E * o0;
+	tVector v1 = E * (v0 - r.cross3(o0));
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyTransF(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E * (o0 - r.cross3(v0));
+	tVector v1 = E * v0;
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyTransM(X, curr_sv);
+	}
+	return result;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyTransF(X, curr_sv);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyInvTransM(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E.transpose() * o0;
+	tVector v1 = E.transpose() * v0 + r.cross3(E.transpose() * o0);
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyInvTransF(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E.transpose() * o0 + r.cross3(E.transpose() * v0);
+	tVector v1 = E.transpose() * v0;
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(sm.rows(), sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyInvTransM(X, curr_sv);
+	}
+	return result;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(sm.rows(), sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyInvTransF(X, curr_sv);
+	}
+	return result;
+}
+
+cSpAlg::tSpTrans cSpAlg::CompTrans(const tSpTrans& X0, const tSpTrans& X1)
+{
+	tMatrix E0 = GetRot(X0);
+	tMatrix E1 = GetRot(X1);
+	tVector r0 = GetRad(X0);
+	tVector r1 = GetRad(X1);
+
+	tSpTrans X = BuildTrans(E0 * E1, r1 + E1.transpose() * r0);
+	return X;
+}
+
+cSpAlg::tSpTrans cSpAlg::GetTrans(const Eigen::MatrixXd& trans_arr, int j)
+{
+	assert(trans_arr.rows() >= gSVTransRows);
+	assert((trans_arr.rows() % gSVTransRows) == 0);
+	assert(trans_arr.cols() == gSVTransCols);
+
+	int row_idx = j * gSVTransRows;
+	assert(row_idx <= trans_arr.rows() - gSVTransRows);
+
+	tSpTrans X = trans_arr.block(row_idx, 0, gSVTransRows, gSVTransCols);
+	return X;
+}

examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "MathUtil.h"
+
+// spatial algebra util
+class cSpAlg
+{
+public:
+	const static int gSpVecSize = 6;
+	const static int gSVTransRows = 3;
+	const static int gSVTransCols = 4;
+
+
+	typedef Eigen::Matrix<double, gSVTransRows, gSVTransCols> tSpTrans;
+	typedef Eigen::Matrix<double, gSpVecSize, 1> tSpVec;
+	typedef Eigen::Matrix<double, gSpVecSize, gSpVecSize> tSpMat;
+
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
+	// rows of R shoold be the basis for the coordinate frame centered ar origin
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
+	// R is a rotation from coord frame 0 to coord frame 1
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
+
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
+
+	static tSpVec CrossM(const tSpVec& sv, const tSpVec& m);
+	static Eigen::MatrixXd CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms);
+	static tSpVec CrossF(const tSpVec& sv, const tSpVec& f);
+	static Eigen::MatrixXd CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs);
+
+	// Spatial Vector methods
+	static tSpVec BuildSV(const tVector& v);
+	static tSpVec BuildSV(const tVector& o, const tVector& v);
+	static tVector GetOmega(const tSpVec& sv);
+	static void SetOmega(const tVector& o, tSpVec& out_sv);
+	static tVector GetV(const tSpVec& sv);
+	static void SetV(const tVector& v, tSpVec& out_sv);
+
+	// tSpTrans methods
+	static tSpTrans BuildTrans();
+	static tSpTrans BuildTrans(const tMatrix& E, const tVector& r);
+	static tSpTrans BuildTrans(const tVector& r);
+
+	static tSpTrans MatToTrans(const tMatrix& mat);
+	static tMatrix TransToMat(const tSpTrans& X);
+	static tSpMat BuildSpatialMatM(const tSpTrans& X);
+	static tSpMat BuildSpatialMatF(const tSpTrans& X);
+	static tSpTrans InvTrans(const tSpTrans& X);
+	static tMatrix GetRot(const tSpTrans& X);
+	static void SetRot(const tMatrix& E, tSpTrans& out_X);
+	static tVector GetRad(const tSpTrans& X);
+	static void SetRad(const tVector& r, tSpTrans& out_X);
+
+	static tSpVec ApplyTransM(const tSpTrans& X, const tSpVec& sv);
+	static tSpVec ApplyTransF(const tSpTrans& X, const tSpVec& sv);
+	static Eigen::MatrixXd ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static Eigen::MatrixXd ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static tSpVec ApplyInvTransM(const tSpTrans& X, const tSpVec& sv);
+	static tSpVec ApplyInvTransF(const tSpTrans& X, const tSpVec& sv);
+	static Eigen::MatrixXd ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static Eigen::MatrixXd ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static tSpTrans CompTrans(const tSpTrans& X0, const tSpTrans& X1);
+	
+	// extract a tSpTrans from a matrix presentating a stack of transforms
+	static tSpTrans GetTrans(const Eigen::MatrixXd& trans_arr, int j);
+};

examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp

@@ -66,6 +66,7 @@ struct TinyRendererVisualShapeConverterInternalData
 	// Maps bodyUniqueId to a list of visual shapes belonging to the body.
 	btHashMap<btHashInt, btAlignedObjectArray<b3VisualShapeData> > m_visualShapesMap;
 
+	int m_uidGenerator;
 	int m_upAxis;
 	int m_swWidth;
 	int m_swHeight;
@@ -92,6 +93,7 @@ struct TinyRendererVisualShapeConverterInternalData
 
 	TinyRendererVisualShapeConverterInternalData()
 		: m_upAxis(2),
+		  m_uidGenerator(1),
 		  m_swWidth(START_WIDTH),
 		  m_swHeight(START_HEIGHT),
 		  m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB),
@@ -606,12 +608,13 @@ static btVector4 sColors[4] =
 		//btVector4(1,1,0,1),
 };
 
-void TinyRendererVisualShapeConverter::convertVisualShapes(
+int  TinyRendererVisualShapeConverter::convertVisualShapes(
 	int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, 
-	const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, 
+	const UrdfLink* linkPtr, const UrdfModel* model, int unused, 
 	int bodyUniqueId, struct CommonFileIOInterface* fileIO)
 
 {
+	int uniqueId = m_data->m_uidGenerator++;
 	btAssert(linkPtr);  // TODO: remove if (not doing it now, because diff will be 50+ lines)
 	if (linkPtr)
 	{
@@ -699,12 +702,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 				}
 			}
 
-			TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
+			TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[uniqueId];
 			if (visualsPtr == 0)
 			{
-				m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new TinyRendererObjectArray);
+				m_data->m_swRenderInstances.insert(uniqueId, new TinyRendererObjectArray);
 			}
-			visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
+			visualsPtr = m_data->m_swRenderInstances[uniqueId];
 
 			btAssert(visualsPtr);
 			TinyRendererObjectArray* visuals = *visualsPtr;
@@ -778,6 +781,8 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 			shapes->push_back(visualShape);
 		}
 	}
+
+	return uniqueId;
 }
 
 int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)

examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h

@@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
 
 	virtual ~TinyRendererVisualShapeConverter();
 
-	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
 
 	virtual int getNumVisualShapes(int bodyUniqueId);
 

examples/ThirdPartyLibs/Eigen/CMakeLists.txt

@@ -0,0 +1,19 @@
+include(RegexUtils)
+test_escape_string_as_regex()
+
+file(GLOB Eigen_directory_files "*")
+
+escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
+
+foreach(f ${Eigen_directory_files})
+  if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src")
+    list(APPEND Eigen_directory_files_to_install ${f})
+  endif()
+endforeach(f ${Eigen_directory_files})
+
+install(FILES
+  ${Eigen_directory_files_to_install}
+  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
+  )
+
+install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h")

examples/ThirdPartyLibs/Eigen/Cholesky

@@ -0,0 +1,46 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CHOLESKY_MODULE_H
+#define EIGEN_CHOLESKY_MODULE_H
+
+#include "Core"
+#include "Jacobi"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup Cholesky_Module Cholesky module
+  *
+  *
+  *
+  * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
+  * Those decompositions are also accessible via the following methods:
+  *  - MatrixBase::llt()
+  *  - MatrixBase::ldlt()
+  *  - SelfAdjointView::llt()
+  *  - SelfAdjointView::ldlt()
+  *
+  * \code
+  * #include <Eigen/Cholesky>
+  * \endcode
+  */
+
+#include "src/Cholesky/LLT.h"
+#include "src/Cholesky/LDLT.h"
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/Cholesky/LLT_LAPACKE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_CHOLESKY_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/CholmodSupport

@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CHOLMODSUPPORT_MODULE_H
+#define EIGEN_CHOLMODSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+extern "C" {
+  #include <cholmod.h>
+}
+
+/** \ingroup Support_modules
+  * \defgroup CholmodSupport_Module CholmodSupport module
+  *
+  * This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
+  * It provides the two following main factorization classes:
+  * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
+  * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).
+  *
+  * For the sake of completeness, this module also propose the two following classes:
+  * - class CholmodSimplicialLLT
+  * - class CholmodSimplicialLDLT
+  * Note that these classes does not bring any particular advantage compared to the built-in
+  * SimplicialLLT and SimplicialLDLT factorization classes.
+  *
+  * \code
+  * #include <Eigen/CholmodSupport>
+  * \endcode
+  *
+  * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies.
+  * The dependencies depend on how cholmod has been compiled.
+  * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task.
+  *
+  */
+
+#include "src/CholmodSupport/CholmodSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_CHOLMODSUPPORT_MODULE_H
+

examples/ThirdPartyLibs/Eigen/Core

@@ -0,0 +1,562 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2007-2011 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CORE_H
+#define EIGEN_CORE_H
+
+// first thing Eigen does: stop the compiler from committing suicide
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
+  #define EIGEN_CUDACC __CUDACC__
+#endif
+
+#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
+  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
+#endif
+
+// Starting with CUDA 9 the composite __CUDACC_VER__ is not available.
+#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
+#define EIGEN_CUDACC_VER  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+#elif defined(__CUDACC_VER__)
+#define EIGEN_CUDACC_VER __CUDACC_VER__
+#else
+#define EIGEN_CUDACC_VER 0
+#endif
+
+// Handle NVCC/CUDA/SYCL
+#if defined(EIGEN_CUDACC) || defined(__SYCL_DEVICE_ONLY__)
+  // Do not try asserts on CUDA and SYCL!
+  #ifndef EIGEN_NO_DEBUG
+  #define EIGEN_NO_DEBUG
+  #endif
+
+  #ifdef EIGEN_INTERNAL_DEBUGGING
+  #undef EIGEN_INTERNAL_DEBUGGING
+  #endif
+
+  #ifdef EIGEN_EXCEPTIONS
+  #undef EIGEN_EXCEPTIONS
+  #endif
+
+  // All functions callable from CUDA code must be qualified with __device__
+  #ifdef EIGEN_CUDACC
+    // Do not try to vectorize on CUDA and SYCL!
+    #ifndef EIGEN_DONT_VECTORIZE
+    #define EIGEN_DONT_VECTORIZE
+    #endif
+
+    #define EIGEN_DEVICE_FUNC __host__ __device__
+    // We need math_functions.hpp to ensure that that EIGEN_USING_STD_MATH macro
+    // works properly on the device side
+    #include <math_functions.hpp>
+  #else
+    #define EIGEN_DEVICE_FUNC
+  #endif
+#else
+  #define EIGEN_DEVICE_FUNC
+#endif
+
+#ifdef __NVCC__
+#define EIGEN_DONT_VECTORIZE
+#endif
+
+// When compiling CUDA device code with NVCC, pull in math functions from the
+// global namespace.  In host mode, and when device doee with clang, use the
+// std versions.
+#if defined(EIGEN_CUDA_ARCH) && defined(__NVCC__)
+  #define EIGEN_USING_STD_MATH(FUNC) using ::FUNC;
+#else
+  #define EIGEN_USING_STD_MATH(FUNC) using std::FUNC;
+#endif
+
+#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_CUDA_ARCH) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL)
+  #define EIGEN_EXCEPTIONS
+#endif
+
+#ifdef EIGEN_EXCEPTIONS
+  #include <new>
+#endif
+
+// then include this file where all our macros are defined. It's really important to do it first because
+// it's where we do all the alignment settings (platform detection and honoring the user's will if he
+// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
+#include "src/Core/util/Macros.h"
+
+// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
+// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
+#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6)
+  #pragma GCC optimize ("-fno-ipa-cp-clone")
+#endif
+
+#include <complex>
+
+// this include file manages BLAS and MKL related macros
+// and inclusion of their respective header files
+#include "src/Core/util/MKL_support.h"
+
+// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
+// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
+#if EIGEN_MAX_ALIGN_BYTES==0
+  #ifndef EIGEN_DONT_VECTORIZE
+    #define EIGEN_DONT_VECTORIZE
+  #endif
+#endif
+
+#if EIGEN_COMP_MSVC
+  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
+  #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
+    // Remember that usage of defined() in a #define is undefined by the standard.
+    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
+    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
+      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
+    #endif
+  #endif
+#else
+  // Remember that usage of defined() in a #define is undefined by the standard
+  #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
+    #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
+  #endif
+#endif
+
+#ifndef EIGEN_DONT_VECTORIZE
+
+  #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
+
+    // Defines symbols for compile-time detection of which instructions are
+    // used.
+    // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_SSE
+    #define EIGEN_VECTORIZE_SSE2
+
+    // Detect sse3/ssse3/sse4:
+    // gcc and icc defines __SSE3__, ...
+    // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
+    // want to force the use of those instructions with msvc.
+    #ifdef __SSE3__
+      #define EIGEN_VECTORIZE_SSE3
+    #endif
+    #ifdef __SSSE3__
+      #define EIGEN_VECTORIZE_SSSE3
+    #endif
+    #ifdef __SSE4_1__
+      #define EIGEN_VECTORIZE_SSE4_1
+    #endif
+    #ifdef __SSE4_2__
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #ifdef __AVX__
+      #define EIGEN_VECTORIZE_AVX
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #ifdef __AVX2__
+      #define EIGEN_VECTORIZE_AVX2
+      #define EIGEN_VECTORIZE_AVX
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #ifdef __FMA__
+      #define EIGEN_VECTORIZE_FMA
+    #endif
+    #if defined(__AVX512F__)
+      #define EIGEN_VECTORIZE_AVX512
+      #define EIGEN_VECTORIZE_AVX2
+      #define EIGEN_VECTORIZE_AVX
+      #define EIGEN_VECTORIZE_FMA
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+      #ifdef __AVX512DQ__
+        #define EIGEN_VECTORIZE_AVX512DQ
+      #endif
+    #endif
+
+    // include files
+
+    // This extern "C" works around a MINGW-w64 compilation issue
+    // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
+    // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
+    // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
+    // with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
+    // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
+    // notice that since these are C headers, the extern "C" is theoretically needed anyways.
+    extern "C" {
+      // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
+      // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
+      #if EIGEN_COMP_ICC >= 1110
+        #include <immintrin.h>
+      #else
+        #include <mmintrin.h>
+        #include <emmintrin.h>
+        #include <xmmintrin.h>
+        #ifdef  EIGEN_VECTORIZE_SSE3
+        #include <pmmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSSE3
+        #include <tmmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSE4_1
+        #include <smmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSE4_2
+        #include <nmmintrin.h>
+        #endif
+        #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
+        #include <immintrin.h>
+        #endif
+      #endif
+    } // end extern "C"
+  #elif defined __VSX__
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_VSX
+    #include <altivec.h>
+    // We need to #undef all these ugly tokens defined in <altivec.h>
+    // => use __vector instead of vector
+    #undef bool
+    #undef vector
+    #undef pixel
+  #elif defined __ALTIVEC__
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_ALTIVEC
+    #include <altivec.h>
+    // We need to #undef all these ugly tokens defined in <altivec.h>
+    // => use __vector instead of vector
+    #undef bool
+    #undef vector
+    #undef pixel
+  #elif (defined  __ARM_NEON) || (defined __ARM_NEON__)
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_NEON
+    #include <arm_neon.h>
+  #elif (defined __s390x__ && defined __VEC__)
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_ZVECTOR
+    #include <vecintrin.h>
+  #endif
+#endif
+
+#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG)
+  // We can use the optimized fp16 to float and float to fp16 conversion routines
+  #define EIGEN_HAS_FP16_C
+#endif
+
+#if defined EIGEN_CUDACC
+  #define EIGEN_VECTORIZE_CUDA
+  #include <vector_types.h>
+  #if EIGEN_CUDACC_VER >= 70500
+    #define EIGEN_HAS_CUDA_FP16
+  #endif
+#endif
+
+#if defined EIGEN_HAS_CUDA_FP16
+  #include <host_defines.h>
+  #include <cuda_fp16.h>
+#endif
+
+#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
+  #define EIGEN_HAS_OPENMP
+#endif
+
+#ifdef EIGEN_HAS_OPENMP
+#include <omp.h>
+#endif
+
+// MSVC for windows mobile does not have the errno.h file
+#if !(EIGEN_COMP_MSVC && EIGEN_OS_WINCE) && !EIGEN_COMP_ARM
+#define EIGEN_HAS_ERRNO
+#endif
+
+#ifdef EIGEN_HAS_ERRNO
+#include <cerrno>
+#endif
+#include <cstddef>
+#include <cstdlib>
+#include <cmath>
+#include <cassert>
+#include <functional>
+#include <iosfwd>
+#include <cstring>
+#include <string>
+#include <limits>
+#include <climits> // for CHAR_BIT
+// for min/max:
+#include <algorithm>
+
+// for std::is_nothrow_move_assignable
+#ifdef EIGEN_INCLUDE_TYPE_TRAITS
+#include <type_traits>
+#endif
+
+// for outputting debug info
+#ifdef EIGEN_DEBUG_ASSIGN
+#include <iostream>
+#endif
+
+// required for __cpuid, needs to be included after cmath
+#if EIGEN_COMP_MSVC && EIGEN_ARCH_i386_OR_x86_64 && !EIGEN_OS_WINCE
+  #include <intrin.h>
+#endif
+
+#if defined(__SYCL_DEVICE_ONLY__)
+  #undef min
+  #undef max
+  #undef isnan
+  #undef isinf
+  #undef isfinite
+  #include <SYCL/sycl.hpp>
+#endif
+
+/** \brief Namespace containing all symbols from the %Eigen library. */
+namespace Eigen {
+
+inline static const char *SimdInstructionSetsInUse(void) {
+#if defined(EIGEN_VECTORIZE_AVX512)
+  return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_AVX)
+  return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_SSE4_2)
+  return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_SSE4_1)
+  return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
+#elif defined(EIGEN_VECTORIZE_SSSE3)
+  return "SSE, SSE2, SSE3, SSSE3";
+#elif defined(EIGEN_VECTORIZE_SSE3)
+  return "SSE, SSE2, SSE3";
+#elif defined(EIGEN_VECTORIZE_SSE2)
+  return "SSE, SSE2";
+#elif defined(EIGEN_VECTORIZE_ALTIVEC)
+  return "AltiVec";
+#elif defined(EIGEN_VECTORIZE_VSX)
+  return "VSX";
+#elif defined(EIGEN_VECTORIZE_NEON)
+  return "ARM NEON";
+#elif defined(EIGEN_VECTORIZE_ZVECTOR)
+  return "S390X ZVECTOR";
+#else
+  return "None";
+#endif
+}
+
+} // end namespace Eigen
+
+#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT
+// This will generate an error message:
+#error Eigen2-support is only available up to version 3.2. Please go to "http://eigen.tuxfamily.org/index.php?title=Eigen2" for further information
+#endif
+
+namespace Eigen {
+
+// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
+// ensure QNX/QCC support
+using std::size_t;
+// gcc 4.6.0 wants std:: for ptrdiff_t
+using std::ptrdiff_t;
+
+}
+
+/** \defgroup Core_Module Core module
+  * This is the main module of Eigen providing dense matrix and vector support
+  * (both fixed and dynamic size) with all the features corresponding to a BLAS library
+  * and much more...
+  *
+  * \code
+  * #include <Eigen/Core>
+  * \endcode
+  */
+
+#include "src/Core/util/Constants.h"
+#include "src/Core/util/Meta.h"
+#include "src/Core/util/ForwardDeclarations.h"
+#include "src/Core/util/StaticAssert.h"
+#include "src/Core/util/XprHelper.h"
+#include "src/Core/util/Memory.h"
+#include "src/Core/util/IntegralConstant.h"
+#include "src/Core/util/SymbolicIndex.h"
+
+
+#include "src/Core/NumTraits.h"
+#include "src/Core/MathFunctions.h"
+#include "src/Core/GenericPacketMath.h"
+#include "src/Core/MathFunctionsImpl.h"
+#include "src/Core/arch/Default/ConjHelper.h"
+
+#if defined EIGEN_VECTORIZE_AVX512
+  #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/AVX/PacketMath.h"
+  #include "src/Core/arch/AVX512/PacketMath.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
+  #include "src/Core/arch/AVX512/MathFunctions.h"
+#elif defined EIGEN_VECTORIZE_AVX
+  // Use AVX for floats and doubles, SSE for integers
+  #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/Complex.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/PacketMath.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
+  #include "src/Core/arch/AVX/Complex.h"
+  #include "src/Core/arch/AVX/TypeCasting.h"
+#elif defined EIGEN_VECTORIZE_SSE
+  #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/SSE/Complex.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
+#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
+  #include "src/Core/arch/AltiVec/PacketMath.h"
+  #include "src/Core/arch/AltiVec/MathFunctions.h"
+  #include "src/Core/arch/AltiVec/Complex.h"
+#elif defined EIGEN_VECTORIZE_NEON
+  #include "src/Core/arch/NEON/PacketMath.h"
+  #include "src/Core/arch/NEON/MathFunctions.h"
+  #include "src/Core/arch/NEON/Complex.h"
+#elif defined EIGEN_VECTORIZE_ZVECTOR
+  #include "src/Core/arch/ZVector/PacketMath.h"
+  #include "src/Core/arch/ZVector/MathFunctions.h"
+  #include "src/Core/arch/ZVector/Complex.h"
+#endif
+
+// Half float support
+#include "src/Core/arch/CUDA/Half.h"
+#include "src/Core/arch/CUDA/PacketMathHalf.h"
+#include "src/Core/arch/CUDA/TypeCasting.h"
+
+#if defined EIGEN_VECTORIZE_CUDA
+  #include "src/Core/arch/CUDA/PacketMath.h"
+  #include "src/Core/arch/CUDA/MathFunctions.h"
+#endif
+
+#include "src/Core/arch/Default/Settings.h"
+
+#include "src/Core/functors/TernaryFunctors.h"
+#include "src/Core/functors/BinaryFunctors.h"
+#include "src/Core/functors/UnaryFunctors.h"
+#include "src/Core/functors/NullaryFunctors.h"
+#include "src/Core/functors/StlFunctors.h"
+#include "src/Core/functors/AssignmentFunctors.h"
+
+// Specialized functors to enable the processing of complex numbers
+// on CUDA devices
+#include "src/Core/arch/CUDA/Complex.h"
+
+#include "src/Core/util/IndexedViewHelper.h"
+#include "src/Core/ArithmeticSequence.h"
+#include "src/Core/IO.h"
+#include "src/Core/DenseCoeffsBase.h"
+#include "src/Core/DenseBase.h"
+#include "src/Core/MatrixBase.h"
+#include "src/Core/EigenBase.h"
+
+#include "src/Core/Product.h"
+#include "src/Core/CoreEvaluators.h"
+#include "src/Core/AssignEvaluator.h"
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
+                                // at least confirmed with Doxygen 1.5.5 and 1.5.6
+  #include "src/Core/Assign.h"
+#endif
+
+#include "src/Core/ArrayBase.h"
+#include "src/Core/util/BlasUtil.h"
+#include "src/Core/DenseStorage.h"
+#include "src/Core/NestByValue.h"
+
+// #include "src/Core/ForceAlignedAccess.h"
+
+#include "src/Core/ReturnByValue.h"
+#include "src/Core/NoAlias.h"
+#include "src/Core/PlainObjectBase.h"
+#include "src/Core/Matrix.h"
+#include "src/Core/Array.h"
+#include "src/Core/CwiseTernaryOp.h"
+#include "src/Core/CwiseBinaryOp.h"
+#include "src/Core/CwiseUnaryOp.h"
+#include "src/Core/CwiseNullaryOp.h"
+#include "src/Core/CwiseUnaryView.h"
+#include "src/Core/SelfCwiseBinaryOp.h"
+#include "src/Core/Dot.h"
+#include "src/Core/StableNorm.h"
+#include "src/Core/Stride.h"
+#include "src/Core/MapBase.h"
+#include "src/Core/Map.h"
+#include "src/Core/Ref.h"
+#include "src/Core/Block.h"
+#include "src/Core/VectorBlock.h"
+#include "src/Core/IndexedView.h"
+#include "src/Core/Transpose.h"
+#include "src/Core/DiagonalMatrix.h"
+#include "src/Core/Diagonal.h"
+#include "src/Core/DiagonalProduct.h"
+#include "src/Core/Redux.h"
+#include "src/Core/Visitor.h"
+#include "src/Core/Fuzzy.h"
+#include "src/Core/Swap.h"
+#include "src/Core/CommaInitializer.h"
+#include "src/Core/GeneralProduct.h"
+#include "src/Core/Solve.h"
+#include "src/Core/Inverse.h"
+#include "src/Core/SolverBase.h"
+#include "src/Core/PermutationMatrix.h"
+#include "src/Core/Transpositions.h"
+#include "src/Core/TriangularMatrix.h"
+#include "src/Core/SelfAdjointView.h"
+#include "src/Core/products/GeneralBlockPanelKernel.h"
+#include "src/Core/products/Parallelizer.h"
+#include "src/Core/ProductEvaluators.h"
+#include "src/Core/products/GeneralMatrixVector.h"
+#include "src/Core/products/GeneralMatrixMatrix.h"
+#include "src/Core/SolveTriangular.h"
+#include "src/Core/products/GeneralMatrixMatrixTriangular.h"
+#include "src/Core/products/SelfadjointMatrixVector.h"
+#include "src/Core/products/SelfadjointMatrixMatrix.h"
+#include "src/Core/products/SelfadjointProduct.h"
+#include "src/Core/products/SelfadjointRank2Update.h"
+#include "src/Core/products/TriangularMatrixVector.h"
+#include "src/Core/products/TriangularMatrixMatrix.h"
+#include "src/Core/products/TriangularSolverMatrix.h"
+#include "src/Core/products/TriangularSolverVector.h"
+#include "src/Core/BandMatrix.h"
+#include "src/Core/CoreIterators.h"
+#include "src/Core/ConditionEstimator.h"
+
+#include "src/Core/BooleanRedux.h"
+#include "src/Core/Select.h"
+#include "src/Core/VectorwiseOp.h"
+#include "src/Core/Random.h"
+#include "src/Core/Replicate.h"
+#include "src/Core/Reverse.h"
+#include "src/Core/ArrayWrapper.h"
+
+#ifdef EIGEN_USE_BLAS
+#include "src/Core/products/GeneralMatrixMatrix_BLAS.h"
+#include "src/Core/products/GeneralMatrixVector_BLAS.h"
+#include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h"
+#include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h"
+#include "src/Core/products/SelfadjointMatrixVector_BLAS.h"
+#include "src/Core/products/TriangularMatrixMatrix_BLAS.h"
+#include "src/Core/products/TriangularMatrixVector_BLAS.h"
+#include "src/Core/products/TriangularSolverMatrix_BLAS.h"
+#endif // EIGEN_USE_BLAS
+
+#ifdef EIGEN_USE_MKL_VML
+#include "src/Core/Assign_MKL.h"
+#endif
+
+#include "src/Core/GlobalFunctions.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_CORE_H

examples/ThirdPartyLibs/Eigen/Dense

@@ -0,0 +1,7 @@
+#include "Core"
+#include "LU"
+#include "Cholesky"
+#include "QR"
+#include "SVD"
+#include "Geometry"
+#include "Eigenvalues"

examples/ThirdPartyLibs/Eigen/Eigen

@@ -0,0 +1,2 @@
+#include "Dense"
+#include "Sparse"

examples/ThirdPartyLibs/Eigen/Eigenvalues

@@ -0,0 +1,61 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_EIGENVALUES_MODULE_H
+#define EIGEN_EIGENVALUES_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "Cholesky"
+#include "Jacobi"
+#include "Householder"
+#include "LU"
+#include "Geometry"
+
+/** \defgroup Eigenvalues_Module Eigenvalues module
+  *
+  *
+  *
+  * This module mainly provides various eigenvalue solvers.
+  * This module also provides some MatrixBase methods, including:
+  *  - MatrixBase::eigenvalues(),
+  *  - MatrixBase::operatorNorm()
+  *
+  * \code
+  * #include <Eigen/Eigenvalues>
+  * \endcode
+  */
+
+#include "src/misc/RealSvd2x2.h"
+#include "src/Eigenvalues/Tridiagonalization.h"
+#include "src/Eigenvalues/RealSchur.h"
+#include "src/Eigenvalues/EigenSolver.h"
+#include "src/Eigenvalues/SelfAdjointEigenSolver.h"
+#include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h"
+#include "src/Eigenvalues/HessenbergDecomposition.h"
+#include "src/Eigenvalues/ComplexSchur.h"
+#include "src/Eigenvalues/ComplexEigenSolver.h"
+#include "src/Eigenvalues/RealQZ.h"
+#include "src/Eigenvalues/GeneralizedEigenSolver.h"
+#include "src/Eigenvalues/MatrixBaseEigenvalues.h"
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/Eigenvalues/RealSchur_LAPACKE.h"
+#include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
+#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_EIGENVALUES_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/Geometry

@@ -0,0 +1,61 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_GEOMETRY_MODULE_H
+#define EIGEN_GEOMETRY_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "SVD"
+#include "LU"
+#include <limits>
+
+/** \defgroup Geometry_Module Geometry module
+  *
+  * This module provides support for:
+  *  - fixed-size homogeneous transformations
+  *  - translation, scaling, 2D and 3D rotations
+  *  - \link Quaternion quaternions \endlink
+  *  - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3)
+  *  - orthognal vector generation (\ref MatrixBase::unitOrthogonal)
+  *  - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink
+  *  - \link AlignedBox axis aligned bounding boxes \endlink
+  *  - \link umeyama least-square transformation fitting \endlink
+  *
+  * \code
+  * #include <Eigen/Geometry>
+  * \endcode
+  */
+
+#include "src/Geometry/OrthoMethods.h"
+#include "src/Geometry/EulerAngles.h"
+
+#include "src/Geometry/Homogeneous.h"
+#include "src/Geometry/RotationBase.h"
+#include "src/Geometry/Rotation2D.h"
+#include "src/Geometry/Quaternion.h"
+#include "src/Geometry/AngleAxis.h"
+#include "src/Geometry/Transform.h"
+#include "src/Geometry/Translation.h"
+#include "src/Geometry/Scaling.h"
+#include "src/Geometry/Hyperplane.h"
+#include "src/Geometry/ParametrizedLine.h"
+#include "src/Geometry/AlignedBox.h"
+#include "src/Geometry/Umeyama.h"
+
+// Use the SSE optimized version whenever possible. At the moment the
+// SSE version doesn't compile when AVX is enabled
+#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
+#include "src/Geometry/arch/Geometry_SSE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_GEOMETRY_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/Householder

@@ -0,0 +1,30 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_HOUSEHOLDER_MODULE_H
+#define EIGEN_HOUSEHOLDER_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup Householder_Module Householder module
+  * This module provides Householder transformations.
+  *
+  * \code
+  * #include <Eigen/Householder>
+  * \endcode
+  */
+
+#include "src/Householder/Householder.h"
+#include "src/Householder/HouseholderSequence.h"
+#include "src/Householder/BlockHouseholder.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_HOUSEHOLDER_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/IterativeLinearSolvers

@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module
+  *
+  * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
+  * Those solvers are accessible via the following classes:
+  *  - ConjugateGradient for selfadjoint (hermitian) matrices,
+  *  - LeastSquaresConjugateGradient for rectangular least-square problems,
+  *  - BiCGSTAB for general square matrices.
+  *
+  * These iterative solvers are associated with some preconditioners:
+  *  - IdentityPreconditioner - not really useful
+  *  - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices.
+  *  - IncompleteLUT - incomplete LU factorization with dual thresholding
+  *
+  * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
+  *
+    \code
+    #include <Eigen/IterativeLinearSolvers>
+    \endcode
+  */
+
+#include "src/IterativeLinearSolvers/SolveWithGuess.h"
+#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
+#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
+#include "src/IterativeLinearSolvers/ConjugateGradient.h"
+#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h"
+#include "src/IterativeLinearSolvers/BiCGSTAB.h"
+#include "src/IterativeLinearSolvers/IncompleteLUT.h"
+#include "src/IterativeLinearSolvers/IncompleteCholesky.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H

examples/ThirdPartyLibs/Eigen/Jacobi

@@ -0,0 +1,33 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_JACOBI_MODULE_H
+#define EIGEN_JACOBI_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup Jacobi_Module Jacobi module
+  * This module provides Jacobi and Givens rotations.
+  *
+  * \code
+  * #include <Eigen/Jacobi>
+  * \endcode
+  *
+  * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation:
+  *  - MatrixBase::applyOnTheLeft()
+  *  - MatrixBase::applyOnTheRight().
+  */
+
+#include "src/Jacobi/Jacobi.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_JACOBI_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */
+

examples/ThirdPartyLibs/Eigen/LU

@@ -0,0 +1,50 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_LU_MODULE_H
+#define EIGEN_LU_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup LU_Module LU module
+  * This module includes %LU decomposition and related notions such as matrix inversion and determinant.
+  * This module defines the following MatrixBase methods:
+  *  - MatrixBase::inverse()
+  *  - MatrixBase::determinant()
+  *
+  * \code
+  * #include <Eigen/LU>
+  * \endcode
+  */
+
+#include "src/misc/Kernel.h"
+#include "src/misc/Image.h"
+#include "src/LU/FullPivLU.h"
+#include "src/LU/PartialPivLU.h"
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/LU/PartialPivLU_LAPACKE.h"
+#endif
+#include "src/LU/Determinant.h"
+#include "src/LU/InverseImpl.h"
+
+// Use the SSE optimized version whenever possible. At the moment the
+// SSE version doesn't compile when AVX is enabled
+#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
+  #include "src/LU/arch/Inverse_SSE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_LU_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/MetisSupport

@@ -0,0 +1,35 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_METISSUPPORT_MODULE_H
+#define EIGEN_METISSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+extern "C" {
+#include <metis.h>
+}
+
+
+/** \ingroup Support_modules
+  * \defgroup MetisSupport_Module MetisSupport module
+  *
+  * \code
+  * #include <Eigen/MetisSupport>
+  * \endcode
+  * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 
+  * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink
+  */
+
+
+#include "src/MetisSupport/MetisSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_METISSUPPORT_MODULE_H

examples/ThirdPartyLibs/Eigen/OrderingMethods

@@ -0,0 +1,73 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ORDERINGMETHODS_MODULE_H
+#define EIGEN_ORDERINGMETHODS_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup OrderingMethods_Module OrderingMethods module
+  *
+  * This module is currently for internal use only
+  * 
+  * It defines various built-in and external ordering methods for sparse matrices. 
+  * They are typically used to reduce the number of elements during 
+  * the sparse matrix decomposition (LLT, LU, QR).
+  * Precisely, in a preprocessing step, a permutation matrix P is computed using 
+  * those ordering methods and applied to the columns of the matrix. 
+  * Using for instance the sparse Cholesky decomposition, it is expected that 
+  * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A).
+  * 
+  * 
+  * Usage : 
+  * \code
+  * #include <Eigen/OrderingMethods>
+  * \endcode
+  * 
+  * A simple usage is as a template parameter in the sparse decomposition classes : 
+  * 
+  * \code 
+  * SparseLU<MatrixType, COLAMDOrdering<int> > solver;
+  * \endcode 
+  * 
+  * \code 
+  * SparseQR<MatrixType, COLAMDOrdering<int> > solver;
+  * \endcode
+  * 
+  * It is possible as well to call directly a particular ordering method for your own purpose, 
+  * \code 
+  * AMDOrdering<int> ordering;
+  * PermutationMatrix<Dynamic, Dynamic, int> perm;
+  * SparseMatrix<double> A; 
+  * //Fill the matrix ...
+  * 
+  * ordering(A, perm); // Call AMD
+  * \endcode
+  * 
+  * \note Some of these methods (like AMD or METIS), need the sparsity pattern 
+  * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 
+  * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method.
+  * If your matrix is already symmetric (at leat in structure), you can avoid that
+  * by calling the method with a SelfAdjointView type.
+  * 
+  * \code
+  *  // Call the ordering on the pattern of the lower triangular matrix A
+  * ordering(A.selfadjointView<Lower>(), perm);
+  * \endcode
+  */
+
+#ifndef EIGEN_MPL2_ONLY
+#include "src/OrderingMethods/Amd.h"
+#endif
+
+#include "src/OrderingMethods/Ordering.h"
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_ORDERINGMETHODS_MODULE_H

examples/ThirdPartyLibs/Eigen/PaStiXSupport

@@ -0,0 +1,48 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_PASTIXSUPPORT_MODULE_H
+#define EIGEN_PASTIXSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+extern "C" {
+#include <pastix_nompi.h>
+#include <pastix.h>
+}
+
+#ifdef complex
+#undef complex
+#endif
+
+/** \ingroup Support_modules
+  * \defgroup PaStiXSupport_Module PaStiXSupport module
+  * 
+  * This module provides an interface to the <a href="http://pastix.gforge.inria.fr/">PaSTiX</a> library.
+  * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver.
+  * It provides the two following main factorization classes:
+  * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization.
+  * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization.
+  * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern).
+  * 
+  * \code
+  * #include <Eigen/PaStiXSupport>
+  * \endcode
+  *
+  * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies.
+  * The dependencies depend on how PaSTiX has been compiled.
+  * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task.
+  *
+  */
+
+#include "src/PaStiXSupport/PaStiXSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_PASTIXSUPPORT_MODULE_H

examples/ThirdPartyLibs/Eigen/PardisoSupport

@@ -0,0 +1,35 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_PARDISOSUPPORT_MODULE_H
+#define EIGEN_PARDISOSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include <mkl_pardiso.h>
+
+/** \ingroup Support_modules
+  * \defgroup PardisoSupport_Module PardisoSupport module
+  *
+  * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers.
+  *
+  * \code
+  * #include <Eigen/PardisoSupport>
+  * \endcode
+  *
+  * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies.
+  * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration.
+  * 
+  */
+
+#include "src/PardisoSupport/PardisoSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_PARDISOSUPPORT_MODULE_H

examples/ThirdPartyLibs/Eigen/QR

@@ -0,0 +1,51 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_QR_MODULE_H
+#define EIGEN_QR_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "Cholesky"
+#include "Jacobi"
+#include "Householder"
+
+/** \defgroup QR_Module QR module
+  *
+  *
+  *
+  * This module provides various QR decompositions
+  * This module also provides some MatrixBase methods, including:
+  *  - MatrixBase::householderQr()
+  *  - MatrixBase::colPivHouseholderQr()
+  *  - MatrixBase::fullPivHouseholderQr()
+  *
+  * \code
+  * #include <Eigen/QR>
+  * \endcode
+  */
+
+#include "src/QR/HouseholderQR.h"
+#include "src/QR/FullPivHouseholderQR.h"
+#include "src/QR/ColPivHouseholderQR.h"
+#include "src/QR/CompleteOrthogonalDecomposition.h"
+#ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/QR/HouseholderQR_LAPACKE.h"
+#include "src/QR/ColPivHouseholderQR_LAPACKE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_QR_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/QtAlignedMalloc

@@ -0,0 +1,40 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_QTMALLOC_MODULE_H
+#define EIGEN_QTMALLOC_MODULE_H
+
+#include "Core"
+
+#if (!EIGEN_MALLOC_ALREADY_ALIGNED)
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+void *qMalloc(std::size_t size)
+{
+  return Eigen::internal::aligned_malloc(size);
+}
+
+void qFree(void *ptr)
+{
+  Eigen::internal::aligned_free(ptr);
+}
+
+void *qRealloc(void *ptr, std::size_t size)
+{
+  void* newPtr = Eigen::internal::aligned_malloc(size);
+  std::memcpy(newPtr, ptr, size);
+  Eigen::internal::aligned_free(ptr);
+  return newPtr;
+}
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif
+
+#endif // EIGEN_QTMALLOC_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/SPQRSupport

@@ -0,0 +1,34 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPQRSUPPORT_MODULE_H
+#define EIGEN_SPQRSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include "SuiteSparseQR.hpp"
+
+/** \ingroup Support_modules
+  * \defgroup SPQRSupport_Module SuiteSparseQR module
+  * 
+  * This module provides an interface to the SPQR library, which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
+  *
+  * \code
+  * #include <Eigen/SPQRSupport>
+  * \endcode
+  *
+  * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...).
+  * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules
+  *
+  */
+
+#include "src/CholmodSupport/CholmodSupport.h"
+#include "src/SPQRSupport/SuiteSparseQRSupport.h"
+
+#endif

examples/ThirdPartyLibs/Eigen/SVD

@@ -0,0 +1,51 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SVD_MODULE_H
+#define EIGEN_SVD_MODULE_H
+
+#include "QR"
+#include "Householder"
+#include "Jacobi"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup SVD_Module SVD module
+  *
+  *
+  *
+  * This module provides SVD decomposition for matrices (both real and complex).
+  * Two decomposition algorithms are provided:
+  *  - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones.
+  *  - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems.
+  * These decompositions are accessible via the respective classes and following MatrixBase methods:
+  *  - MatrixBase::jacobiSvd()
+  *  - MatrixBase::bdcSvd()
+  *
+  * \code
+  * #include <Eigen/SVD>
+  * \endcode
+  */
+
+#include "src/misc/RealSvd2x2.h"
+#include "src/SVD/UpperBidiagonalization.h"
+#include "src/SVD/SVDBase.h"
+#include "src/SVD/JacobiSVD.h"
+#include "src/SVD/BDCSVD.h"
+#if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
+#include "src/misc/lapacke.h"
+#endif
+#include "src/SVD/JacobiSVD_LAPACKE.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SVD_MODULE_H
+/* vim: set filetype=cpp et sw=2 ts=2 ai: */

examples/ThirdPartyLibs/Eigen/Sparse

@@ -0,0 +1,36 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPARSE_MODULE_H
+#define EIGEN_SPARSE_MODULE_H
+
+/** \defgroup Sparse_Module Sparse meta-module
+  *
+  * Meta-module including all related modules:
+  * - \ref SparseCore_Module
+  * - \ref OrderingMethods_Module
+  * - \ref SparseCholesky_Module
+  * - \ref SparseLU_Module
+  * - \ref SparseQR_Module
+  * - \ref IterativeLinearSolvers_Module
+  *
+    \code
+    #include <Eigen/Sparse>
+    \endcode
+  */
+
+#include "SparseCore"
+#include "OrderingMethods"
+#ifndef EIGEN_MPL2_ONLY
+#include "SparseCholesky"
+#endif
+#include "SparseLU"
+#include "SparseQR"
+#include "IterativeLinearSolvers"
+
+#endif // EIGEN_SPARSE_MODULE_H
+

examples/ThirdPartyLibs/Eigen/SparseCholesky

@@ -0,0 +1,45 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPARSECHOLESKY_MODULE_H
+#define EIGEN_SPARSECHOLESKY_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** 
+  * \defgroup SparseCholesky_Module SparseCholesky module
+  *
+  * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices.
+  * Those decompositions are accessible via the following classes:
+  *  - SimplicialLLt,
+  *  - SimplicialLDLt
+  *
+  * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module.
+  *
+  * \code
+  * #include <Eigen/SparseCholesky>
+  * \endcode
+  */
+
+#ifdef EIGEN_MPL2_ONLY
+#error The SparseCholesky module has nothing to offer in MPL2 only mode
+#endif
+
+#include "src/SparseCholesky/SimplicialCholesky.h"
+
+#ifndef EIGEN_MPL2_ONLY
+#include "src/SparseCholesky/SimplicialCholesky_impl.h"
+#endif
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECHOLESKY_MODULE_H

examples/ThirdPartyLibs/Eigen/SparseCore

@@ -0,0 +1,69 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPARSECORE_MODULE_H
+#define EIGEN_SPARSECORE_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <algorithm>
+
+/** 
+  * \defgroup SparseCore_Module SparseCore module
+  *
+  * This module provides a sparse matrix representation, and basic associated matrix manipulations
+  * and operations.
+  *
+  * See the \ref TutorialSparse "Sparse tutorial"
+  *
+  * \code
+  * #include <Eigen/SparseCore>
+  * \endcode
+  *
+  * This module depends on: Core.
+  */
+
+#include "src/SparseCore/SparseUtil.h"
+#include "src/SparseCore/SparseMatrixBase.h"
+#include "src/SparseCore/SparseAssign.h"
+#include "src/SparseCore/CompressedStorage.h"
+#include "src/SparseCore/AmbiVector.h"
+#include "src/SparseCore/SparseCompressedBase.h"
+#include "src/SparseCore/SparseMatrix.h"
+#include "src/SparseCore/SparseMap.h"
+#include "src/SparseCore/MappedSparseMatrix.h"
+#include "src/SparseCore/SparseVector.h"
+#include "src/SparseCore/SparseRef.h"
+#include "src/SparseCore/SparseCwiseUnaryOp.h"
+#include "src/SparseCore/SparseCwiseBinaryOp.h"
+#include "src/SparseCore/SparseTranspose.h"
+#include "src/SparseCore/SparseBlock.h"
+#include "src/SparseCore/SparseDot.h"
+#include "src/SparseCore/SparseRedux.h"
+#include "src/SparseCore/SparseView.h"
+#include "src/SparseCore/SparseDiagonalProduct.h"
+#include "src/SparseCore/ConservativeSparseSparseProduct.h"
+#include "src/SparseCore/SparseSparseProductWithPruning.h"
+#include "src/SparseCore/SparseProduct.h"
+#include "src/SparseCore/SparseDenseProduct.h"
+#include "src/SparseCore/SparseSelfAdjointView.h"
+#include "src/SparseCore/SparseTriangularView.h"
+#include "src/SparseCore/TriangularSolver.h"
+#include "src/SparseCore/SparsePermutation.h"
+#include "src/SparseCore/SparseFuzzy.h"
+#include "src/SparseCore/SparseSolverBase.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SPARSECORE_MODULE_H
+

examples/ThirdPartyLibs/Eigen/SparseLU

@@ -0,0 +1,46 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2012 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPARSELU_MODULE_H
+#define EIGEN_SPARSELU_MODULE_H
+
+#include "SparseCore"
+
+/** 
+  * \defgroup SparseLU_Module SparseLU module
+  * This module defines a supernodal factorization of general sparse matrices.
+  * The code is fully optimized for supernode-panel updates with specialized kernels.
+  * Please, see the documentation of the SparseLU class for more details.
+  */
+
+// Ordering interface
+#include "OrderingMethods"
+
+#include "src/SparseLU/SparseLU_gemm_kernel.h"
+
+#include "src/SparseLU/SparseLU_Structs.h"
+#include "src/SparseLU/SparseLU_SupernodalMatrix.h"
+#include "src/SparseLU/SparseLUImpl.h"
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseLU/SparseLU_Memory.h"
+#include "src/SparseLU/SparseLU_heap_relax_snode.h"
+#include "src/SparseLU/SparseLU_relax_snode.h"
+#include "src/SparseLU/SparseLU_pivotL.h"
+#include "src/SparseLU/SparseLU_panel_dfs.h"
+#include "src/SparseLU/SparseLU_kernel_bmod.h"
+#include "src/SparseLU/SparseLU_panel_bmod.h"
+#include "src/SparseLU/SparseLU_column_dfs.h"
+#include "src/SparseLU/SparseLU_column_bmod.h"
+#include "src/SparseLU/SparseLU_copy_to_ucol.h"
+#include "src/SparseLU/SparseLU_pruneL.h"
+#include "src/SparseLU/SparseLU_Utils.h"
+#include "src/SparseLU/SparseLU.h"
+
+#endif // EIGEN_SPARSELU_MODULE_H

examples/ThirdPartyLibs/Eigen/SparseQR

@@ -0,0 +1,37 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SPARSEQR_MODULE_H
+#define EIGEN_SPARSEQR_MODULE_H
+
+#include "SparseCore"
+#include "OrderingMethods"
+#include "src/Core/util/DisableStupidWarnings.h"
+
+/** \defgroup SparseQR_Module SparseQR module
+  * \brief Provides QR decomposition for sparse matrices
+  * 
+  * This module provides a simplicial version of the left-looking Sparse QR decomposition. 
+  * The columns of the input matrix should be reordered to limit the fill-in during the 
+  * decomposition. Built-in methods (COLAMD, AMD) or external  methods (METIS) can be used to this end.
+  * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 
+  * of built-in and external ordering methods.
+  * 
+  * \code
+  * #include <Eigen/SparseQR>
+  * \endcode
+  * 
+  * 
+  */
+
+#include "OrderingMethods"
+#include "src/SparseCore/SparseColEtree.h"
+#include "src/SparseQR/SparseQR.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif

examples/ThirdPartyLibs/Eigen/StdDeque

@@ -0,0 +1,27 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_STDDEQUE_MODULE_H
+#define EIGEN_STDDEQUE_MODULE_H
+
+#include "Core"
+#include <deque>
+
+#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
+
+#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...)
+
+#else
+
+#include "src/StlSupport/StdDeque.h"
+
+#endif
+
+#endif // EIGEN_STDDEQUE_MODULE_H

examples/ThirdPartyLibs/Eigen/StdList

@@ -0,0 +1,26 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_STDLIST_MODULE_H
+#define EIGEN_STDLIST_MODULE_H
+
+#include "Core"
+#include <list>
+
+#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
+
+#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...)
+
+#else
+
+#include "src/StlSupport/StdList.h"
+
+#endif
+
+#endif // EIGEN_STDLIST_MODULE_H

examples/ThirdPartyLibs/Eigen/StdVector

@@ -0,0 +1,27 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_STDVECTOR_MODULE_H
+#define EIGEN_STDVECTOR_MODULE_H
+
+#include "Core"
+#include <vector>
+
+#if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
+
+#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
+
+#else
+
+#include "src/StlSupport/StdVector.h"
+
+#endif
+
+#endif // EIGEN_STDVECTOR_MODULE_H

examples/ThirdPartyLibs/Eigen/SuperLUSupport

@@ -0,0 +1,64 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SUPERLUSUPPORT_MODULE_H
+#define EIGEN_SUPERLUSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+#ifdef EMPTY
+#define EIGEN_EMPTY_WAS_ALREADY_DEFINED
+#endif
+
+typedef int int_t;
+#include <slu_Cnames.h>
+#include <supermatrix.h>
+#include <slu_util.h>
+
+// slu_util.h defines a preprocessor token named EMPTY which is really polluting,
+// so we remove it in favor of a SUPERLU_EMPTY token.
+// If EMPTY was already defined then we don't undef it.
+
+#if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED)
+# undef EIGEN_EMPTY_WAS_ALREADY_DEFINED
+#elif defined(EMPTY)
+# undef EMPTY
+#endif
+
+#define SUPERLU_EMPTY (-1)
+
+namespace Eigen { struct SluMatrix; }
+
+/** \ingroup Support_modules
+  * \defgroup SuperLUSupport_Module SuperLUSupport module
+  *
+  * This module provides an interface to the <a href="http://crd-legacy.lbl.gov/~xiaoye/SuperLU/">SuperLU</a> library.
+  * It provides the following factorization class:
+  * - class SuperLU: a supernodal sequential LU factorization.
+  * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods).
+  *
+  * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported.
+  *
+  * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting.
+  *
+  * \code
+  * #include <Eigen/SuperLUSupport>
+  * \endcode
+  *
+  * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies.
+  * The dependencies depend on how superlu has been compiled.
+  * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task.
+  *
+  */
+
+#include "src/SuperLUSupport/SuperLUSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_SUPERLUSUPPORT_MODULE_H

examples/ThirdPartyLibs/Eigen/UmfPackSupport

@@ -0,0 +1,40 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_UMFPACKSUPPORT_MODULE_H
+#define EIGEN_UMFPACKSUPPORT_MODULE_H
+
+#include "SparseCore"
+
+#include "src/Core/util/DisableStupidWarnings.h"
+
+extern "C" {
+#include <umfpack.h>
+}
+
+/** \ingroup Support_modules
+  * \defgroup UmfPackSupport_Module UmfPackSupport module
+  *
+  * This module provides an interface to the UmfPack library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
+  * It provides the following factorization class:
+  * - class UmfPackLU: a multifrontal sequential LU factorization.
+  *
+  * \code
+  * #include <Eigen/UmfPackSupport>
+  * \endcode
+  *
+  * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies.
+  * The dependencies depend on how umfpack has been compiled.
+  * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task.
+  *
+  */
+
+#include "src/UmfPackSupport/UmfPackSupport.h"
+
+#include "src/Core/util/ReenableStupidWarnings.h"
+
+#endif // EIGEN_UMFPACKSUPPORT_MODULE_H

examples/ThirdPartyLibs/Eigen/src/Cholesky/LDLT.h

@@ -0,0 +1,668 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2009 Keir Mierle <mierle@gmail.com>
+// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2011 Timothy E. Holy <tim.holy@gmail.com >
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_LDLT_H
+#define EIGEN_LDLT_H
+
+namespace Eigen {
+
+namespace internal {
+  template<typename MatrixType, int UpLo> struct LDLT_Traits;
+
+  // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
+  enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
+}
+
+/** \ingroup Cholesky_Module
+  *
+  * \class LDLT
+  *
+  * \brief Robust Cholesky decomposition of a matrix with pivoting
+  *
+  * \tparam _MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition
+  * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
+  *             The other triangular part won't be read.
+  *
+  * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
+  * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
+  * is lower triangular with a unit diagonal and D is a diagonal matrix.
+  *
+  * The decomposition uses pivoting to ensure stability, so that L will have
+  * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
+  * on D also stabilizes the computation.
+  *
+  * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
+  * decomposition to determine whether a system of equations has a solution.
+  *
+  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+  * 
+  * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT
+  */
+template<typename _MatrixType, int _UpLo> class LDLT
+{
+  public:
+    typedef _MatrixType MatrixType;
+    enum {
+      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
+      UpLo = _UpLo
+    };
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    typedef Matrix<Scalar, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime, 1> TmpMatrixType;
+
+    typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
+    typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
+
+    typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
+
+    /** \brief Default Constructor.
+      *
+      * The default constructor is useful in cases in which the user intends to
+      * perform decompositions via LDLT::compute(const MatrixType&).
+      */
+    LDLT()
+      : m_matrix(),
+        m_transpositions(),
+        m_sign(internal::ZeroSign),
+        m_isInitialized(false)
+    {}
+
+    /** \brief Default Constructor with memory preallocation
+      *
+      * Like the default constructor but with preallocation of the internal data
+      * according to the specified problem \a size.
+      * \sa LDLT()
+      */
+    explicit LDLT(Index size)
+      : m_matrix(size, size),
+        m_transpositions(size),
+        m_temporary(size),
+        m_sign(internal::ZeroSign),
+        m_isInitialized(false)
+    {}
+
+    /** \brief Constructor with decomposition
+      *
+      * This calculates the decomposition for the input \a matrix.
+      *
+      * \sa LDLT(Index size)
+      */
+    template<typename InputType>
+    explicit LDLT(const EigenBase<InputType>& matrix)
+      : m_matrix(matrix.rows(), matrix.cols()),
+        m_transpositions(matrix.rows()),
+        m_temporary(matrix.rows()),
+        m_sign(internal::ZeroSign),
+        m_isInitialized(false)
+    {
+      compute(matrix.derived());
+    }
+
+    /** \brief Constructs a LDLT factorization from a given matrix
+      *
+      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
+      *
+      * \sa LDLT(const EigenBase&)
+      */
+    template<typename InputType>
+    explicit LDLT(EigenBase<InputType>& matrix)
+      : m_matrix(matrix.derived()),
+        m_transpositions(matrix.rows()),
+        m_temporary(matrix.rows()),
+        m_sign(internal::ZeroSign),
+        m_isInitialized(false)
+    {
+      compute(matrix.derived());
+    }
+
+    /** Clear any existing decomposition
+     * \sa rankUpdate(w,sigma)
+     */
+    void setZero()
+    {
+      m_isInitialized = false;
+    }
+
+    /** \returns a view of the upper triangular matrix U */
+    inline typename Traits::MatrixU matrixU() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return Traits::getU(m_matrix);
+    }
+
+    /** \returns a view of the lower triangular matrix L */
+    inline typename Traits::MatrixL matrixL() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return Traits::getL(m_matrix);
+    }
+
+    /** \returns the permutation matrix P as a transposition sequence.
+      */
+    inline const TranspositionType& transpositionsP() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_transpositions;
+    }
+
+    /** \returns the coefficients of the diagonal matrix D */
+    inline Diagonal<const MatrixType> vectorD() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_matrix.diagonal();
+    }
+
+    /** \returns true if the matrix is positive (semidefinite) */
+    inline bool isPositive() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
+    }
+
+    /** \returns true if the matrix is negative (semidefinite) */
+    inline bool isNegative(void) const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
+    }
+
+    /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
+      *
+      * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
+      *
+      * \note_about_checking_solutions
+      *
+      * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$
+      * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$,
+      * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
+      * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
+      * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
+      * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular.
+      *
+      * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt()
+      */
+    template<typename Rhs>
+    inline const Solve<LDLT, Rhs>
+    solve(const MatrixBase<Rhs>& b) const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      eigen_assert(m_matrix.rows()==b.rows()
+                && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
+      return Solve<LDLT, Rhs>(*this, b.derived());
+    }
+
+    template<typename Derived>
+    bool solveInPlace(MatrixBase<Derived> &bAndX) const;
+
+    template<typename InputType>
+    LDLT& compute(const EigenBase<InputType>& matrix);
+
+    /** \returns an estimate of the reciprocal condition number of the matrix of
+     *  which \c *this is the LDLT decomposition.
+     */
+    RealScalar rcond() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return internal::rcond_estimate_helper(m_l1_norm, *this);
+    }
+
+    template <typename Derived>
+    LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha=1);
+
+    /** \returns the internal LDLT decomposition matrix
+      *
+      * TODO: document the storage layout
+      */
+    inline const MatrixType& matrixLDLT() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_matrix;
+    }
+
+    MatrixType reconstructedMatrix() const;
+
+    /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint.
+      *
+      * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
+      * \code x = decomposition.adjoint().solve(b) \endcode
+      */
+    const LDLT& adjoint() const { return *this; };
+
+    inline Index rows() const { return m_matrix.rows(); }
+    inline Index cols() const { return m_matrix.cols(); }
+
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was succesful,
+      *          \c NumericalIssue if the factorization failed because of a zero pivot.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "LDLT is not initialized.");
+      return m_info;
+    }
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename RhsType, typename DstType>
+    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+    #endif
+
+  protected:
+
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+
+    /** \internal
+      * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
+      * The strict upper part is used during the decomposition, the strict lower
+      * part correspond to the coefficients of L (its diagonal is equal to 1 and
+      * is not stored), and the diagonal entries correspond to D.
+      */
+    MatrixType m_matrix;
+    RealScalar m_l1_norm;
+    TranspositionType m_transpositions;
+    TmpMatrixType m_temporary;
+    internal::SignMatrix m_sign;
+    bool m_isInitialized;
+    ComputationInfo m_info;
+};
+
+namespace internal {
+
+template<int UpLo> struct ldlt_inplace;
+
+template<> struct ldlt_inplace<Lower>
+{
+  template<typename MatrixType, typename TranspositionType, typename Workspace>
+  static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
+  {
+    using std::abs;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename TranspositionType::StorageIndex IndexType;
+    eigen_assert(mat.rows()==mat.cols());
+    const Index size = mat.rows();
+    bool found_zero_pivot = false;
+    bool ret = true;
+
+    if (size <= 1)
+    {
+      transpositions.setIdentity();
+      if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
+      else if (numext::real(mat.coeff(0,0)) < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
+      else sign = ZeroSign;
+      return true;
+    }
+
+    for (Index k = 0; k < size; ++k)
+    {
+      // Find largest diagonal element
+      Index index_of_biggest_in_corner;
+      mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
+      index_of_biggest_in_corner += k;
+
+      transpositions.coeffRef(k) = IndexType(index_of_biggest_in_corner);
+      if(k != index_of_biggest_in_corner)
+      {
+        // apply the transposition while taking care to consider only
+        // the lower triangular part
+        Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
+        mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
+        mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
+        std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
+        for(Index i=k+1;i<index_of_biggest_in_corner;++i)
+        {
+          Scalar tmp = mat.coeffRef(i,k);
+          mat.coeffRef(i,k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner,i));
+          mat.coeffRef(index_of_biggest_in_corner,i) = numext::conj(tmp);
+        }
+        if(NumTraits<Scalar>::IsComplex)
+          mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k));
+      }
+
+      // partition the matrix:
+      //       A00 |  -  |  -
+      // lu  = A10 | A11 |  -
+      //       A20 | A21 | A22
+      Index rs = size - k - 1;
+      Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
+      Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
+      Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
+
+      if(k>0)
+      {
+        temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
+        mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
+        if(rs>0)
+          A21.noalias() -= A20 * temp.head(k);
+      }
+
+      // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
+      // was smaller than the cutoff value. However, since LDLT is not rank-revealing
+      // we should only make sure that we do not introduce INF or NaN values.
+      // Remark that LAPACK also uses 0 as the cutoff value.
+      RealScalar realAkk = numext::real(mat.coeffRef(k,k));
+      bool pivot_is_valid = (abs(realAkk) > RealScalar(0));
+
+      if(k==0 && !pivot_is_valid)
+      {
+        // The entire diagonal is zero, there is nothing more to do
+        // except filling the transpositions, and checking whether the matrix is zero.
+        sign = ZeroSign;
+        for(Index j = 0; j<size; ++j)
+        {
+          transpositions.coeffRef(j) = IndexType(j);
+          ret = ret && (mat.col(j).tail(size-j-1).array()==Scalar(0)).all();
+        }
+        return ret;
+      }
+
+      if((rs>0) && pivot_is_valid)
+        A21 /= realAkk;
+
+      if(found_zero_pivot && pivot_is_valid) ret = false; // factorization failed
+      else if(!pivot_is_valid) found_zero_pivot = true;
+
+      if (sign == PositiveSemiDef) {
+        if (realAkk < static_cast<RealScalar>(0)) sign = Indefinite;
+      } else if (sign == NegativeSemiDef) {
+        if (realAkk > static_cast<RealScalar>(0)) sign = Indefinite;
+      } else if (sign == ZeroSign) {
+        if (realAkk > static_cast<RealScalar>(0)) sign = PositiveSemiDef;
+        else if (realAkk < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
+      }
+    }
+
+    return ret;
+  }
+
+  // Reference for the algorithm: Davis and Hager, "Multiple Rank
+  // Modifications of a Sparse Cholesky Factorization" (Algorithm 1)
+  // Trivial rearrangements of their computations (Timothy E. Holy)
+  // allow their algorithm to work for rank-1 updates even if the
+  // original matrix is not of full rank.
+  // Here only rank-1 updates are implemented, to reduce the
+  // requirement for intermediate storage and improve accuracy
+  template<typename MatrixType, typename WDerived>
+  static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w, const typename MatrixType::RealScalar& sigma=1)
+  {
+    using numext::isfinite;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+
+    const Index size = mat.rows();
+    eigen_assert(mat.cols() == size && w.size()==size);
+
+    RealScalar alpha = 1;
+
+    // Apply the update
+    for (Index j = 0; j < size; j++)
+    {
+      // Check for termination due to an original decomposition of low-rank
+      if (!(isfinite)(alpha))
+        break;
+
+      // Update the diagonal terms
+      RealScalar dj = numext::real(mat.coeff(j,j));
+      Scalar wj = w.coeff(j);
+      RealScalar swj2 = sigma*numext::abs2(wj);
+      RealScalar gamma = dj*alpha + swj2;
+
+      mat.coeffRef(j,j) += swj2/alpha;
+      alpha += swj2/dj;
+
+
+      // Update the terms of L
+      Index rs = size-j-1;
+      w.tail(rs) -= wj * mat.col(j).tail(rs);
+      if(gamma != 0)
+        mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs);
+    }
+    return true;
+  }
+
+  template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
+  static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1)
+  {
+    // Apply the permutation to the input w
+    tmp = transpositions * w;
+
+    return ldlt_inplace<Lower>::updateInPlace(mat,tmp,sigma);
+  }
+};
+
+template<> struct ldlt_inplace<Upper>
+{
+  template<typename MatrixType, typename TranspositionType, typename Workspace>
+  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
+  {
+    Transpose<MatrixType> matt(mat);
+    return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
+  }
+
+  template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
+  static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1)
+  {
+    Transpose<MatrixType> matt(mat);
+    return ldlt_inplace<Lower>::update(matt, transpositions, tmp, w.conjugate(), sigma);
+  }
+};
+
+template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
+{
+  typedef const TriangularView<const MatrixType, UnitLower> MatrixL;
+  typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
+  static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); }
+  static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); }
+};
+
+template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
+{
+  typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
+  typedef const TriangularView<const MatrixType, UnitUpper> MatrixU;
+  static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); }
+  static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); }
+};
+
+} // end namespace internal
+
+/** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
+  */
+template<typename MatrixType, int _UpLo>
+template<typename InputType>
+LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>& a)
+{
+  check_template_parameters();
+
+  eigen_assert(a.rows()==a.cols());
+  const Index size = a.rows();
+
+  m_matrix = a.derived();
+
+  // Compute matrix L1 norm = max abs column sum.
+  m_l1_norm = RealScalar(0);
+  // TODO move this code to SelfAdjointView
+  for (Index col = 0; col < size; ++col) {
+    RealScalar abs_col_sum;
+    if (_UpLo == Lower)
+      abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
+    else
+      abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
+    if (abs_col_sum > m_l1_norm)
+      m_l1_norm = abs_col_sum;
+  }
+
+  m_transpositions.resize(size);
+  m_isInitialized = false;
+  m_temporary.resize(size);
+  m_sign = internal::ZeroSign;
+
+  m_info = internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign) ? Success : NumericalIssue;
+
+  m_isInitialized = true;
+  return *this;
+}
+
+/** Update the LDLT decomposition:  given A = L D L^T, efficiently compute the decomposition of A + sigma w w^T.
+ * \param w a vector to be incorporated into the decomposition.
+ * \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column vectors. Optional; default value is +1.
+ * \sa setZero()
+  */
+template<typename MatrixType, int _UpLo>
+template<typename Derived>
+LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename LDLT<MatrixType,_UpLo>::RealScalar& sigma)
+{
+  typedef typename TranspositionType::StorageIndex IndexType;
+  const Index size = w.rows();
+  if (m_isInitialized)
+  {
+    eigen_assert(m_matrix.rows()==size);
+  }
+  else
+  {
+    m_matrix.resize(size,size);
+    m_matrix.setZero();
+    m_transpositions.resize(size);
+    for (Index i = 0; i < size; i++)
+      m_transpositions.coeffRef(i) = IndexType(i);
+    m_temporary.resize(size);
+    m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
+    m_isInitialized = true;
+  }
+
+  internal::ldlt_inplace<UpLo>::update(m_matrix, m_transpositions, m_temporary, w, sigma);
+
+  return *this;
+}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename _MatrixType, int _UpLo>
+template<typename RhsType, typename DstType>
+void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
+{
+  eigen_assert(rhs.rows() == rows());
+  // dst = P b
+  dst = m_transpositions * rhs;
+
+  // dst = L^-1 (P b)
+  matrixL().solveInPlace(dst);
+
+  // dst = D^-1 (L^-1 P b)
+  // more precisely, use pseudo-inverse of D (see bug 241)
+  using std::abs;
+  const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
+  // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
+  // as motivated by LAPACK's xGELSS:
+  // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
+  // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
+  // diagonal element is not well justified and leads to numerical issues in some cases.
+  // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
+  RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
+
+  for (Index i = 0; i < vecD.size(); ++i)
+  {
+    if(abs(vecD(i)) > tolerance)
+      dst.row(i) /= vecD(i);
+    else
+      dst.row(i).setZero();
+  }
+
+  // dst = L^-T (D^-1 L^-1 P b)
+  matrixU().solveInPlace(dst);
+
+  // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
+  dst = m_transpositions.transpose() * dst;
+}
+#endif
+
+/** \internal use x = ldlt_object.solve(x);
+  *
+  * This is the \em in-place version of solve().
+  *
+  * \param bAndX represents both the right-hand side matrix b and result x.
+  *
+  * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
+  *
+  * This version avoids a copy when the right hand side matrix b is not
+  * needed anymore.
+  *
+  * \sa LDLT::solve(), MatrixBase::ldlt()
+  */
+template<typename MatrixType,int _UpLo>
+template<typename Derived>
+bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
+{
+  eigen_assert(m_isInitialized && "LDLT is not initialized.");
+  eigen_assert(m_matrix.rows() == bAndX.rows());
+
+  bAndX = this->solve(bAndX);
+
+  return true;
+}
+
+/** \returns the matrix represented by the decomposition,
+ * i.e., it returns the product: P^T L D L^* P.
+ * This function is provided for debug purpose. */
+template<typename MatrixType, int _UpLo>
+MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
+{
+  eigen_assert(m_isInitialized && "LDLT is not initialized.");
+  const Index size = m_matrix.rows();
+  MatrixType res(size,size);
+
+  // P
+  res.setIdentity();
+  res = transpositionsP() * res;
+  // L^* P
+  res = matrixU() * res;
+  // D(L^*P)
+  res = vectorD().real().asDiagonal() * res;
+  // L(DL^*P)
+  res = matrixL() * res;
+  // P^T (LDL^*P)
+  res = transpositionsP().transpose() * res;
+
+  return res;
+}
+
+/** \cholesky_module
+  * \returns the Cholesky decomposition with full pivoting without square root of \c *this
+  * \sa MatrixBase::ldlt()
+  */
+template<typename MatrixType, unsigned int UpLo>
+inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
+SelfAdjointView<MatrixType, UpLo>::ldlt() const
+{
+  return LDLT<PlainObject,UpLo>(m_matrix);
+}
+
+/** \cholesky_module
+  * \returns the Cholesky decomposition with full pivoting without square root of \c *this
+  * \sa SelfAdjointView::ldlt()
+  */
+template<typename Derived>
+inline const LDLT<typename MatrixBase<Derived>::PlainObject>
+MatrixBase<Derived>::ldlt() const
+{
+  return LDLT<PlainObject>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_LDLT_H

examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT.h

@@ -0,0 +1,541 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_LLT_H
+#define EIGEN_LLT_H
+
+namespace Eigen {
+
+namespace internal{
+template<typename MatrixType, int UpLo> struct LLT_Traits;
+}
+
+/** \ingroup Cholesky_Module
+  *
+  * \class LLT
+  *
+  * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features
+  *
+  * \tparam _MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
+  * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
+  *               The other triangular part won't be read.
+  *
+  * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
+  * matrix A such that A = LL^* = U^*U, where L is lower triangular.
+  *
+  * While the Cholesky decomposition is particularly useful to solve selfadjoint problems like  D^*D x = b,
+  * for that purpose, we recommend the Cholesky decomposition without square root which is more stable
+  * and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other
+  * situations like generalised eigen problems with hermitian matrices.
+  *
+  * Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive definite matrices,
+  * use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine whether a system of equations
+  * has a solution.
+  *
+  * Example: \include LLT_example.cpp
+  * Output: \verbinclude LLT_example.out
+  *
+  * \b Performance: for best performance, it is recommended to use a column-major storage format
+  * with the Lower triangular part (the default), or, equivalently, a row-major storage format
+  * with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization
+  * step, and rank-updates can be up to 3 times slower.
+  *
+  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
+  *
+  * Note that during the decomposition, only the lower (or upper, as defined by _UpLo) triangular part of A is considered.
+  * Therefore, the strict lower part does not have to store correct values.
+  *
+  * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
+  */
+template<typename _MatrixType, int _UpLo> class LLT
+{
+  public:
+    typedef _MatrixType MatrixType;
+    enum {
+      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+    };
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
+    typedef typename MatrixType::StorageIndex StorageIndex;
+
+    enum {
+      PacketSize = internal::packet_traits<Scalar>::size,
+      AlignmentMask = int(PacketSize)-1,
+      UpLo = _UpLo
+    };
+
+    typedef internal::LLT_Traits<MatrixType,UpLo> Traits;
+
+    /**
+      * \brief Default Constructor.
+      *
+      * The default constructor is useful in cases in which the user intends to
+      * perform decompositions via LLT::compute(const MatrixType&).
+      */
+    LLT() : m_matrix(), m_isInitialized(false) {}
+
+    /** \brief Default Constructor with memory preallocation
+      *
+      * Like the default constructor but with preallocation of the internal data
+      * according to the specified problem \a size.
+      * \sa LLT()
+      */
+    explicit LLT(Index size) : m_matrix(size, size),
+                    m_isInitialized(false) {}
+
+    template<typename InputType>
+    explicit LLT(const EigenBase<InputType>& matrix)
+      : m_matrix(matrix.rows(), matrix.cols()),
+        m_isInitialized(false)
+    {
+      compute(matrix.derived());
+    }
+
+    /** \brief Constructs a LDLT factorization from a given matrix
+      *
+      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
+      * \c MatrixType is a Eigen::Ref.
+      *
+      * \sa LLT(const EigenBase&)
+      */
+    template<typename InputType>
+    explicit LLT(EigenBase<InputType>& matrix)
+      : m_matrix(matrix.derived()),
+        m_isInitialized(false)
+    {
+      compute(matrix.derived());
+    }
+
+    /** \returns a view of the upper triangular matrix U */
+    inline typename Traits::MatrixU matrixU() const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      return Traits::getU(m_matrix);
+    }
+
+    /** \returns a view of the lower triangular matrix L */
+    inline typename Traits::MatrixL matrixL() const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      return Traits::getL(m_matrix);
+    }
+
+    /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
+      *
+      * Since this LLT class assumes anyway that the matrix A is invertible, the solution
+      * theoretically exists and is unique regardless of b.
+      *
+      * Example: \include LLT_solve.cpp
+      * Output: \verbinclude LLT_solve.out
+      *
+      * \sa solveInPlace(), MatrixBase::llt(), SelfAdjointView::llt()
+      */
+    template<typename Rhs>
+    inline const Solve<LLT, Rhs>
+    solve(const MatrixBase<Rhs>& b) const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      eigen_assert(m_matrix.rows()==b.rows()
+                && "LLT::solve(): invalid number of rows of the right hand side matrix b");
+      return Solve<LLT, Rhs>(*this, b.derived());
+    }
+
+    template<typename Derived>
+    void solveInPlace(const MatrixBase<Derived> &bAndX) const;
+
+    template<typename InputType>
+    LLT& compute(const EigenBase<InputType>& matrix);
+
+    /** \returns an estimate of the reciprocal condition number of the matrix of
+      *  which \c *this is the Cholesky decomposition.
+      */
+    RealScalar rcond() const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      eigen_assert(m_info == Success && "LLT failed because matrix appears to be negative");
+      return internal::rcond_estimate_helper(m_l1_norm, *this);
+    }
+
+    /** \returns the LLT decomposition matrix
+      *
+      * TODO: document the storage layout
+      */
+    inline const MatrixType& matrixLLT() const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      return m_matrix;
+    }
+
+    MatrixType reconstructedMatrix() const;
+
+
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was succesful,
+      *          \c NumericalIssue if the matrix.appears not to be positive definite.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "LLT is not initialized.");
+      return m_info;
+    }
+
+    /** \returns the adjoint of \c *this, that is, a const reference to the decomposition itself as the underlying matrix is self-adjoint.
+      *
+      * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
+      * \code x = decomposition.adjoint().solve(b) \endcode
+      */
+    const LLT& adjoint() const { return *this; };
+
+    inline Index rows() const { return m_matrix.rows(); }
+    inline Index cols() const { return m_matrix.cols(); }
+
+    template<typename VectorType>
+    LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename RhsType, typename DstType>
+    void _solve_impl(const RhsType &rhs, DstType &dst) const;
+    #endif
+
+  protected:
+
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+
+    /** \internal
+      * Used to compute and store L
+      * The strict upper part is not used and even not initialized.
+      */
+    MatrixType m_matrix;
+    RealScalar m_l1_norm;
+    bool m_isInitialized;
+    ComputationInfo m_info;
+};
+
+namespace internal {
+
+template<typename Scalar, int UpLo> struct llt_inplace;
+
+template<typename MatrixType, typename VectorType>
+static Index llt_rank_update_lower(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma)
+{
+  using std::sqrt;
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  typedef typename MatrixType::ColXpr ColXpr;
+  typedef typename internal::remove_all<ColXpr>::type ColXprCleaned;
+  typedef typename ColXprCleaned::SegmentReturnType ColXprSegment;
+  typedef Matrix<Scalar,Dynamic,1> TempVectorType;
+  typedef typename TempVectorType::SegmentReturnType TempVecSegment;
+
+  Index n = mat.cols();
+  eigen_assert(mat.rows()==n && vec.size()==n);
+
+  TempVectorType temp;
+
+  if(sigma>0)
+  {
+    // This version is based on Givens rotations.
+    // It is faster than the other one below, but only works for updates,
+    // i.e., for sigma > 0
+    temp = sqrt(sigma) * vec;
+
+    for(Index i=0; i<n; ++i)
+    {
+      JacobiRotation<Scalar> g;
+      g.makeGivens(mat(i,i), -temp(i), &mat(i,i));
+
+      Index rs = n-i-1;
+      if(rs>0)
+      {
+        ColXprSegment x(mat.col(i).tail(rs));
+        TempVecSegment y(temp.tail(rs));
+        apply_rotation_in_the_plane(x, y, g);
+      }
+    }
+  }
+  else
+  {
+    temp = vec;
+    RealScalar beta = 1;
+    for(Index j=0; j<n; ++j)
+    {
+      RealScalar Ljj = numext::real(mat.coeff(j,j));
+      RealScalar dj = numext::abs2(Ljj);
+      Scalar wj = temp.coeff(j);
+      RealScalar swj2 = sigma*numext::abs2(wj);
+      RealScalar gamma = dj*beta + swj2;
+
+      RealScalar x = dj + swj2/beta;
+      if (x<=RealScalar(0))
+        return j;
+      RealScalar nLjj = sqrt(x);
+      mat.coeffRef(j,j) = nLjj;
+      beta += swj2/dj;
+
+      // Update the terms of L
+      Index rs = n-j-1;
+      if(rs)
+      {
+        temp.tail(rs) -= (wj/Ljj) * mat.col(j).tail(rs);
+        if(gamma != 0)
+          mat.col(j).tail(rs) = (nLjj/Ljj) * mat.col(j).tail(rs) + (nLjj * sigma*numext::conj(wj)/gamma)*temp.tail(rs);
+      }
+    }
+  }
+  return -1;
+}
+
+template<typename Scalar> struct llt_inplace<Scalar, Lower>
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  template<typename MatrixType>
+  static Index unblocked(MatrixType& mat)
+  {
+    using std::sqrt;
+
+    eigen_assert(mat.rows()==mat.cols());
+    const Index size = mat.rows();
+    for(Index k = 0; k < size; ++k)
+    {
+      Index rs = size-k-1; // remaining size
+
+      Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
+      Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
+      Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
+
+      RealScalar x = numext::real(mat.coeff(k,k));
+      if (k>0) x -= A10.squaredNorm();
+      if (x<=RealScalar(0))
+        return k;
+      mat.coeffRef(k,k) = x = sqrt(x);
+      if (k>0 && rs>0) A21.noalias() -= A20 * A10.adjoint();
+      if (rs>0) A21 /= x;
+    }
+    return -1;
+  }
+
+  template<typename MatrixType>
+  static Index blocked(MatrixType& m)
+  {
+    eigen_assert(m.rows()==m.cols());
+    Index size = m.rows();
+    if(size<32)
+      return unblocked(m);
+
+    Index blockSize = size/8;
+    blockSize = (blockSize/16)*16;
+    blockSize = (std::min)((std::max)(blockSize,Index(8)), Index(128));
+
+    for (Index k=0; k<size; k+=blockSize)
+    {
+      // partition the matrix:
+      //       A00 |  -  |  -
+      // lu  = A10 | A11 |  -
+      //       A20 | A21 | A22
+      Index bs = (std::min)(blockSize, size-k);
+      Index rs = size - k - bs;
+      Block<MatrixType,Dynamic,Dynamic> A11(m,k,   k,   bs,bs);
+      Block<MatrixType,Dynamic,Dynamic> A21(m,k+bs,k,   rs,bs);
+      Block<MatrixType,Dynamic,Dynamic> A22(m,k+bs,k+bs,rs,rs);
+
+      Index ret;
+      if((ret=unblocked(A11))>=0) return k+ret;
+      if(rs>0) A11.adjoint().template triangularView<Upper>().template solveInPlace<OnTheRight>(A21);
+      if(rs>0) A22.template selfadjointView<Lower>().rankUpdate(A21,typename NumTraits<RealScalar>::Literal(-1)); // bottleneck
+    }
+    return -1;
+  }
+
+  template<typename MatrixType, typename VectorType>
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma)
+  {
+    return Eigen::internal::llt_rank_update_lower(mat, vec, sigma);
+  }
+};
+
+template<typename Scalar> struct llt_inplace<Scalar, Upper>
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+
+  template<typename MatrixType>
+  static EIGEN_STRONG_INLINE Index unblocked(MatrixType& mat)
+  {
+    Transpose<MatrixType> matt(mat);
+    return llt_inplace<Scalar, Lower>::unblocked(matt);
+  }
+  template<typename MatrixType>
+  static EIGEN_STRONG_INLINE Index blocked(MatrixType& mat)
+  {
+    Transpose<MatrixType> matt(mat);
+    return llt_inplace<Scalar, Lower>::blocked(matt);
+  }
+  template<typename MatrixType, typename VectorType>
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const RealScalar& sigma)
+  {
+    Transpose<MatrixType> matt(mat);
+    return llt_inplace<Scalar, Lower>::rankUpdate(matt, vec.conjugate(), sigma);
+  }
+};
+
+template<typename MatrixType> struct LLT_Traits<MatrixType,Lower>
+{
+  typedef const TriangularView<const MatrixType, Lower> MatrixL;
+  typedef const TriangularView<const typename MatrixType::AdjointReturnType, Upper> MatrixU;
+  static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); }
+  static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); }
+  static bool inplace_decomposition(MatrixType& m)
+  { return llt_inplace<typename MatrixType::Scalar, Lower>::blocked(m)==-1; }
+};
+
+template<typename MatrixType> struct LLT_Traits<MatrixType,Upper>
+{
+  typedef const TriangularView<const typename MatrixType::AdjointReturnType, Lower> MatrixL;
+  typedef const TriangularView<const MatrixType, Upper> MatrixU;
+  static inline MatrixL getL(const MatrixType& m) { return MatrixL(m.adjoint()); }
+  static inline MatrixU getU(const MatrixType& m) { return MatrixU(m); }
+  static bool inplace_decomposition(MatrixType& m)
+  { return llt_inplace<typename MatrixType::Scalar, Upper>::blocked(m)==-1; }
+};
+
+} // end namespace internal
+
+/** Computes / recomputes the Cholesky decomposition A = LL^* = U^*U of \a matrix
+  *
+  * \returns a reference to *this
+  *
+  * Example: \include TutorialLinAlgComputeTwice.cpp
+  * Output: \verbinclude TutorialLinAlgComputeTwice.out
+  */
+template<typename MatrixType, int _UpLo>
+template<typename InputType>
+LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>& a)
+{
+  check_template_parameters();
+
+  eigen_assert(a.rows()==a.cols());
+  const Index size = a.rows();
+  m_matrix.resize(size, size);
+  if (!internal::is_same_dense(m_matrix, a.derived()))
+    m_matrix = a.derived();
+
+  // Compute matrix L1 norm = max abs column sum.
+  m_l1_norm = RealScalar(0);
+  // TODO move this code to SelfAdjointView
+  for (Index col = 0; col < size; ++col) {
+    RealScalar abs_col_sum;
+    if (_UpLo == Lower)
+      abs_col_sum = m_matrix.col(col).tail(size - col).template lpNorm<1>() + m_matrix.row(col).head(col).template lpNorm<1>();
+    else
+      abs_col_sum = m_matrix.col(col).head(col).template lpNorm<1>() + m_matrix.row(col).tail(size - col).template lpNorm<1>();
+    if (abs_col_sum > m_l1_norm)
+      m_l1_norm = abs_col_sum;
+  }
+
+  m_isInitialized = true;
+  bool ok = Traits::inplace_decomposition(m_matrix);
+  m_info = ok ? Success : NumericalIssue;
+
+  return *this;
+}
+
+/** Performs a rank one update (or dowdate) of the current decomposition.
+  * If A = LL^* before the rank one update,
+  * then after it we have LL^* = A + sigma * v v^* where \a v must be a vector
+  * of same dimension.
+  */
+template<typename _MatrixType, int _UpLo>
+template<typename VectorType>
+LLT<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType);
+  eigen_assert(v.size()==m_matrix.cols());
+  eigen_assert(m_isInitialized);
+  if(internal::llt_inplace<typename MatrixType::Scalar, UpLo>::rankUpdate(m_matrix,v,sigma)>=0)
+    m_info = NumericalIssue;
+  else
+    m_info = Success;
+
+  return *this;
+}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+template<typename _MatrixType,int _UpLo>
+template<typename RhsType, typename DstType>
+void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
+{
+  dst = rhs;
+  solveInPlace(dst);
+}
+#endif
+
+/** \internal use x = llt_object.solve(x);
+  *
+  * This is the \em in-place version of solve().
+  *
+  * \param bAndX represents both the right-hand side matrix b and result x.
+  *
+  * This version avoids a copy when the right hand side matrix b is not needed anymore.
+  *
+  * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+  * This function will const_cast it, so constness isn't honored here.
+  *
+  * \sa LLT::solve(), MatrixBase::llt()
+  */
+template<typename MatrixType, int _UpLo>
+template<typename Derived>
+void LLT<MatrixType,_UpLo>::solveInPlace(const MatrixBase<Derived> &bAndX) const
+{
+  eigen_assert(m_isInitialized && "LLT is not initialized.");
+  eigen_assert(m_matrix.rows()==bAndX.rows());
+  matrixL().solveInPlace(bAndX);
+  matrixU().solveInPlace(bAndX);
+}
+
+/** \returns the matrix represented by the decomposition,
+ * i.e., it returns the product: L L^*.
+ * This function is provided for debug purpose. */
+template<typename MatrixType, int _UpLo>
+MatrixType LLT<MatrixType,_UpLo>::reconstructedMatrix() const
+{
+  eigen_assert(m_isInitialized && "LLT is not initialized.");
+  return matrixL() * matrixL().adjoint().toDenseMatrix();
+}
+
+/** \cholesky_module
+  * \returns the LLT decomposition of \c *this
+  * \sa SelfAdjointView::llt()
+  */
+template<typename Derived>
+inline const LLT<typename MatrixBase<Derived>::PlainObject>
+MatrixBase<Derived>::llt() const
+{
+  return LLT<PlainObject>(derived());
+}
+
+/** \cholesky_module
+  * \returns the LLT decomposition of \c *this
+  * \sa SelfAdjointView::llt()
+  */
+template<typename MatrixType, unsigned int UpLo>
+inline const LLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
+SelfAdjointView<MatrixType, UpLo>::llt() const
+{
+  return LLT<PlainObject,UpLo>(m_matrix);
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_LLT_H

examples/ThirdPartyLibs/Eigen/src/Cholesky/LLT_LAPACKE.h

@@ -0,0 +1,99 @@
+/*
+ Copyright (c) 2011, Intel Corporation. All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without modification,
+ are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+ * Neither the name of Intel Corporation nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ ********************************************************************************
+ *   Content : Eigen bindings to LAPACKe
+ *     LLt decomposition based on LAPACKE_?potrf function.
+ ********************************************************************************
+*/
+
+#ifndef EIGEN_LLT_LAPACKE_H
+#define EIGEN_LLT_LAPACKE_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Scalar> struct lapacke_llt;
+
+#define EIGEN_LAPACKE_LLT(EIGTYPE, BLASTYPE, LAPACKE_PREFIX) \
+template<> struct lapacke_llt<EIGTYPE> \
+{ \
+  template<typename MatrixType> \
+  static inline Index potrf(MatrixType& m, char uplo) \
+  { \
+    lapack_int matrix_order; \
+    lapack_int size, lda, info, StorageOrder; \
+    EIGTYPE* a; \
+    eigen_assert(m.rows()==m.cols()); \
+    /* Set up parameters for ?potrf */ \
+    size = convert_index<lapack_int>(m.rows()); \
+    StorageOrder = MatrixType::Flags&RowMajorBit?RowMajor:ColMajor; \
+    matrix_order = StorageOrder==RowMajor ? LAPACK_ROW_MAJOR : LAPACK_COL_MAJOR; \
+    a = &(m.coeffRef(0,0)); \
+    lda = convert_index<lapack_int>(m.outerStride()); \
+\
+    info = LAPACKE_##LAPACKE_PREFIX##potrf( matrix_order, uplo, size, (BLASTYPE*)a, lda ); \
+    info = (info==0) ? -1 : info>0 ? info-1 : size; \
+    return info; \
+  } \
+}; \
+template<> struct llt_inplace<EIGTYPE, Lower> \
+{ \
+  template<typename MatrixType> \
+  static Index blocked(MatrixType& m) \
+  { \
+    return lapacke_llt<EIGTYPE>::potrf(m, 'L'); \
+  } \
+  template<typename MatrixType, typename VectorType> \
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \
+  { return Eigen::internal::llt_rank_update_lower(mat, vec, sigma); } \
+}; \
+template<> struct llt_inplace<EIGTYPE, Upper> \
+{ \
+  template<typename MatrixType> \
+  static Index blocked(MatrixType& m) \
+  { \
+    return lapacke_llt<EIGTYPE>::potrf(m, 'U'); \
+  } \
+  template<typename MatrixType, typename VectorType> \
+  static Index rankUpdate(MatrixType& mat, const VectorType& vec, const typename MatrixType::RealScalar& sigma) \
+  { \
+    Transpose<MatrixType> matt(mat); \
+    return llt_inplace<EIGTYPE, Lower>::rankUpdate(matt, vec.conjugate(), sigma); \
+  } \
+};
+
+EIGEN_LAPACKE_LLT(double, double, d)
+EIGEN_LAPACKE_LLT(float, float, s)
+EIGEN_LAPACKE_LLT(dcomplex, lapack_complex_double, z)
+EIGEN_LAPACKE_LLT(scomplex, lapack_complex_float, c)
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_LLT_LAPACKE_H

examples/ThirdPartyLibs/Eigen/src/CholmodSupport/CholmodSupport.h

@@ -0,0 +1,682 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CHOLMODSUPPORT_H
+#define EIGEN_CHOLMODSUPPORT_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Scalar> struct cholmod_configure_matrix;
+
+template<> struct cholmod_configure_matrix<double> {
+  template<typename CholmodType>
+  static void run(CholmodType& mat) {
+    mat.xtype = CHOLMOD_REAL;
+    mat.dtype = CHOLMOD_DOUBLE;
+  }
+};
+
+template<> struct cholmod_configure_matrix<std::complex<double> > {
+  template<typename CholmodType>
+  static void run(CholmodType& mat) {
+    mat.xtype = CHOLMOD_COMPLEX;
+    mat.dtype = CHOLMOD_DOUBLE;
+  }
+};
+
+// Other scalar types are not yet supported by Cholmod
+// template<> struct cholmod_configure_matrix<float> {
+//   template<typename CholmodType>
+//   static void run(CholmodType& mat) {
+//     mat.xtype = CHOLMOD_REAL;
+//     mat.dtype = CHOLMOD_SINGLE;
+//   }
+// };
+//
+// template<> struct cholmod_configure_matrix<std::complex<float> > {
+//   template<typename CholmodType>
+//   static void run(CholmodType& mat) {
+//     mat.xtype = CHOLMOD_COMPLEX;
+//     mat.dtype = CHOLMOD_SINGLE;
+//   }
+// };
+
+} // namespace internal
+
+/** Wraps the Eigen sparse matrix \a mat into a Cholmod sparse matrix object.
+  * Note that the data are shared.
+  */
+template<typename _Scalar, int _Options, typename _StorageIndex>
+cholmod_sparse viewAsCholmod(Ref<SparseMatrix<_Scalar,_Options,_StorageIndex> > mat)
+{
+  cholmod_sparse res;
+  res.nzmax   = mat.nonZeros();
+  res.nrow    = mat.rows();
+  res.ncol    = mat.cols();
+  res.p       = mat.outerIndexPtr();
+  res.i       = mat.innerIndexPtr();
+  res.x       = mat.valuePtr();
+  res.z       = 0;
+  res.sorted  = 1;
+  if(mat.isCompressed())
+  {
+    res.packed  = 1;
+    res.nz = 0;
+  }
+  else
+  {
+    res.packed  = 0;
+    res.nz = mat.innerNonZeroPtr();
+  }
+
+  res.dtype   = 0;
+  res.stype   = -1;
+  
+  if (internal::is_same<_StorageIndex,int>::value)
+  {
+    res.itype = CHOLMOD_INT;
+  }
+  else if (internal::is_same<_StorageIndex,long>::value)
+  {
+    res.itype = CHOLMOD_LONG;
+  }
+  else
+  {
+    eigen_assert(false && "Index type not supported yet");
+  }
+
+  // setup res.xtype
+  internal::cholmod_configure_matrix<_Scalar>::run(res);
+  
+  res.stype = 0;
+  
+  return res;
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+const cholmod_sparse viewAsCholmod(const SparseMatrix<_Scalar,_Options,_Index>& mat)
+{
+  cholmod_sparse res = viewAsCholmod(Ref<SparseMatrix<_Scalar,_Options,_Index> >(mat.const_cast_derived()));
+  return res;
+}
+
+template<typename _Scalar, int _Options, typename _Index>
+const cholmod_sparse viewAsCholmod(const SparseVector<_Scalar,_Options,_Index>& mat)
+{
+  cholmod_sparse res = viewAsCholmod(Ref<SparseMatrix<_Scalar,_Options,_Index> >(mat.const_cast_derived()));
+  return res;
+}
+
+/** Returns a view of the Eigen sparse matrix \a mat as Cholmod sparse matrix.
+  * The data are not copied but shared. */
+template<typename _Scalar, int _Options, typename _Index, unsigned int UpLo>
+cholmod_sparse viewAsCholmod(const SparseSelfAdjointView<const SparseMatrix<_Scalar,_Options,_Index>, UpLo>& mat)
+{
+  cholmod_sparse res = viewAsCholmod(Ref<SparseMatrix<_Scalar,_Options,_Index> >(mat.matrix().const_cast_derived()));
+  
+  if(UpLo==Upper) res.stype =  1;
+  if(UpLo==Lower) res.stype = -1;
+  // swap stype for rowmajor matrices (only works for real matrices)
+  EIGEN_STATIC_ASSERT((_Options & RowMajorBit) == 0 || NumTraits<_Scalar>::IsComplex == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  if(_Options & RowMajorBit) res.stype *=-1;
+
+  return res;
+}
+
+/** Returns a view of the Eigen \b dense matrix \a mat as Cholmod dense matrix.
+  * The data are not copied but shared. */
+template<typename Derived>
+cholmod_dense viewAsCholmod(MatrixBase<Derived>& mat)
+{
+  EIGEN_STATIC_ASSERT((internal::traits<Derived>::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  typedef typename Derived::Scalar Scalar;
+
+  cholmod_dense res;
+  res.nrow   = mat.rows();
+  res.ncol   = mat.cols();
+  res.nzmax  = res.nrow * res.ncol;
+  res.d      = Derived::IsVectorAtCompileTime ? mat.derived().size() : mat.derived().outerStride();
+  res.x      = (void*)(mat.derived().data());
+  res.z      = 0;
+
+  internal::cholmod_configure_matrix<Scalar>::run(res);
+
+  return res;
+}
+
+/** Returns a view of the Cholmod sparse matrix \a cm as an Eigen sparse matrix.
+  * The data are not copied but shared. */
+template<typename Scalar, int Flags, typename StorageIndex>
+MappedSparseMatrix<Scalar,Flags,StorageIndex> viewAsEigen(cholmod_sparse& cm)
+{
+  return MappedSparseMatrix<Scalar,Flags,StorageIndex>
+         (cm.nrow, cm.ncol, static_cast<StorageIndex*>(cm.p)[cm.ncol],
+          static_cast<StorageIndex*>(cm.p), static_cast<StorageIndex*>(cm.i),static_cast<Scalar*>(cm.x) );
+}
+
+namespace internal {
+
+// template specializations for int and long that call the correct cholmod method
+
+#define EIGEN_CHOLMOD_SPECIALIZE0(ret, name) \
+    template<typename _StorageIndex> inline ret cm_ ## name       (cholmod_common &Common) { return cholmod_ ## name   (&Common); } \
+    template<>                       inline ret cm_ ## name<long> (cholmod_common &Common) { return cholmod_l_ ## name (&Common); }
+
+#define EIGEN_CHOLMOD_SPECIALIZE1(ret, name, t1, a1) \
+    template<typename _StorageIndex> inline ret cm_ ## name       (t1& a1, cholmod_common &Common) { return cholmod_ ## name   (&a1, &Common); } \
+    template<>                       inline ret cm_ ## name<long> (t1& a1, cholmod_common &Common) { return cholmod_l_ ## name (&a1, &Common); }
+
+EIGEN_CHOLMOD_SPECIALIZE0(int, start)
+EIGEN_CHOLMOD_SPECIALIZE0(int, finish)
+
+EIGEN_CHOLMOD_SPECIALIZE1(int, free_factor, cholmod_factor*, L)
+EIGEN_CHOLMOD_SPECIALIZE1(int, free_dense,  cholmod_dense*,  X)
+EIGEN_CHOLMOD_SPECIALIZE1(int, free_sparse, cholmod_sparse*, A)
+
+EIGEN_CHOLMOD_SPECIALIZE1(cholmod_factor*, analyze, cholmod_sparse, A)
+
+template<typename _StorageIndex> inline cholmod_dense*  cm_solve         (int sys, cholmod_factor& L, cholmod_dense&  B, cholmod_common &Common) { return cholmod_solve     (sys, &L, &B, &Common); }
+template<>                       inline cholmod_dense*  cm_solve<long>   (int sys, cholmod_factor& L, cholmod_dense&  B, cholmod_common &Common) { return cholmod_l_solve   (sys, &L, &B, &Common); }
+
+template<typename _StorageIndex> inline cholmod_sparse* cm_spsolve       (int sys, cholmod_factor& L, cholmod_sparse& B, cholmod_common &Common) { return cholmod_spsolve   (sys, &L, &B, &Common); }
+template<>                       inline cholmod_sparse* cm_spsolve<long> (int sys, cholmod_factor& L, cholmod_sparse& B, cholmod_common &Common) { return cholmod_l_spsolve (sys, &L, &B, &Common); }
+
+template<typename _StorageIndex>
+inline int  cm_factorize_p       (cholmod_sparse*  A, double beta[2], _StorageIndex* fset, std::size_t fsize, cholmod_factor* L, cholmod_common &Common) { return cholmod_factorize_p   (A, beta, fset, fsize, L, &Common); }
+template<>
+inline int  cm_factorize_p<long> (cholmod_sparse*  A, double beta[2], long* fset,          std::size_t fsize, cholmod_factor* L, cholmod_common &Common) { return cholmod_l_factorize_p (A, beta, fset, fsize, L, &Common); }
+
+#undef EIGEN_CHOLMOD_SPECIALIZE0
+#undef EIGEN_CHOLMOD_SPECIALIZE1
+
+}  // namespace internal
+
+
+enum CholmodMode {
+  CholmodAuto, CholmodSimplicialLLt, CholmodSupernodalLLt, CholmodLDLt
+};
+
+
+/** \ingroup CholmodSupport_Module
+  * \class CholmodBase
+  * \brief The base class for the direct Cholesky factorization of Cholmod
+  * \sa class CholmodSupernodalLLT, class CholmodSimplicialLDLT, class CholmodSimplicialLLT
+  */
+template<typename _MatrixType, int _UpLo, typename Derived>
+class CholmodBase : public SparseSolverBase<Derived>
+{
+  protected:
+    typedef SparseSolverBase<Derived> Base;
+    using Base::derived;
+    using Base::m_isInitialized;
+  public:
+    typedef _MatrixType MatrixType;
+    enum { UpLo = _UpLo };
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef MatrixType CholMatrixType;
+    typedef typename MatrixType::StorageIndex StorageIndex;
+    enum {
+      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+    };
+
+  public:
+
+    CholmodBase()
+      : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false)
+    {
+      EIGEN_STATIC_ASSERT((internal::is_same<double,RealScalar>::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY);
+      m_shiftOffset[0] = m_shiftOffset[1] = 0.0;
+      internal::cm_start<StorageIndex>(m_cholmod);
+    }
+
+    explicit CholmodBase(const MatrixType& matrix)
+      : m_cholmodFactor(0), m_info(Success), m_factorizationIsOk(false), m_analysisIsOk(false)
+    {
+      EIGEN_STATIC_ASSERT((internal::is_same<double,RealScalar>::value), CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY);
+      m_shiftOffset[0] = m_shiftOffset[1] = 0.0;
+      internal::cm_start<StorageIndex>(m_cholmod);
+      compute(matrix);
+    }
+
+    ~CholmodBase()
+    {
+      if(m_cholmodFactor)
+        internal::cm_free_factor<StorageIndex>(m_cholmodFactor, m_cholmod);
+      internal::cm_finish<StorageIndex>(m_cholmod);
+    }
+    
+    inline StorageIndex cols() const { return internal::convert_index<StorageIndex, Index>(m_cholmodFactor->n); }
+    inline StorageIndex rows() const { return internal::convert_index<StorageIndex, Index>(m_cholmodFactor->n); }
+    
+    /** \brief Reports whether previous computation was successful.
+      *
+      * \returns \c Success if computation was successful,
+      *          \c NumericalIssue if the matrix.appears to be negative.
+      */
+    ComputationInfo info() const
+    {
+      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
+      return m_info;
+    }
+
+    /** Computes the sparse Cholesky decomposition of \a matrix */
+    Derived& compute(const MatrixType& matrix)
+    {
+      analyzePattern(matrix);
+      factorize(matrix);
+      return derived();
+    }
+    
+    /** Performs a symbolic decomposition on the sparsity pattern of \a matrix.
+      *
+      * This function is particularly useful when solving for several problems having the same structure.
+      * 
+      * \sa factorize()
+      */
+    void analyzePattern(const MatrixType& matrix)
+    {
+      if(m_cholmodFactor)
+      {
+        internal::cm_free_factor<StorageIndex>(m_cholmodFactor, m_cholmod);
+        m_cholmodFactor = 0;
+      }
+      cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView<UpLo>());
+      m_cholmodFactor = internal::cm_analyze<StorageIndex>(A, m_cholmod);
+      
+      this->m_isInitialized = true;
+      this->m_info = Success;
+      m_analysisIsOk = true;
+      m_factorizationIsOk = false;
+    }
+    
+    /** Performs a numeric decomposition of \a matrix
+      *
+      * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed.
+      *
+      * \sa analyzePattern()
+      */
+    void factorize(const MatrixType& matrix)
+    {
+      eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
+      cholmod_sparse A = viewAsCholmod(matrix.template selfadjointView<UpLo>());
+      internal::cm_factorize_p<StorageIndex>(&A, m_shiftOffset, 0, 0, m_cholmodFactor, m_cholmod);
+
+      // If the factorization failed, minor is the column at which it did. On success minor == n.
+      this->m_info = (m_cholmodFactor->minor == m_cholmodFactor->n ? Success : NumericalIssue);
+      m_factorizationIsOk = true;
+    }
+    
+    /** Returns a reference to the Cholmod's configuration structure to get a full control over the performed operations.
+     *  See the Cholmod user guide for details. */
+    cholmod_common& cholmod() { return m_cholmod; }
+    
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal */
+    template<typename Rhs,typename Dest>
+    void _solve_impl(const MatrixBase<Rhs> &b, MatrixBase<Dest> &dest) const
+    {
+      eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+      const Index size = m_cholmodFactor->n;
+      EIGEN_UNUSED_VARIABLE(size);
+      eigen_assert(size==b.rows());
+      
+      // Cholmod needs column-major storage without inner-stride, which corresponds to the default behavior of Ref.
+      Ref<const Matrix<typename Rhs::Scalar,Dynamic,Dynamic,ColMajor> > b_ref(b.derived());
+
+      cholmod_dense b_cd = viewAsCholmod(b_ref);
+      cholmod_dense* x_cd = internal::cm_solve<StorageIndex>(CHOLMOD_A, *m_cholmodFactor, b_cd, m_cholmod);
+      if(!x_cd)
+      {
+        this->m_info = NumericalIssue;
+        return;
+      }
+      // TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
+      // NOTE Actually, the copy can be avoided by calling cholmod_solve2 instead of cholmod_solve
+      dest = Matrix<Scalar,Dest::RowsAtCompileTime,Dest::ColsAtCompileTime>::Map(reinterpret_cast<Scalar*>(x_cd->x),b.rows(),b.cols());
+      internal::cm_free_dense<StorageIndex>(x_cd, m_cholmod);
+    }
+    
+    /** \internal */
+    template<typename RhsDerived, typename DestDerived>
+    void _solve_impl(const SparseMatrixBase<RhsDerived> &b, SparseMatrixBase<DestDerived> &dest) const
+    {
+      eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+      const Index size = m_cholmodFactor->n;
+      EIGEN_UNUSED_VARIABLE(size);
+      eigen_assert(size==b.rows());
+
+      // note: cs stands for Cholmod Sparse
+      Ref<SparseMatrix<typename RhsDerived::Scalar,ColMajor,typename RhsDerived::StorageIndex> > b_ref(b.const_cast_derived());
+      cholmod_sparse b_cs = viewAsCholmod(b_ref);
+      cholmod_sparse* x_cs = internal::cm_spsolve<StorageIndex>(CHOLMOD_A, *m_cholmodFactor, b_cs, m_cholmod);
+      if(!x_cs)
+      {
+        this->m_info = NumericalIssue;
+        return;
+      }
+      // TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
+      // NOTE cholmod_spsolve in fact just calls the dense solver for blocks of 4 columns at a time (similar to Eigen's sparse solver)
+      dest.derived() = viewAsEigen<typename DestDerived::Scalar,ColMajor,typename DestDerived::StorageIndex>(*x_cs);
+      internal::cm_free_sparse<StorageIndex>(x_cs, m_cholmod);
+    }
+    #endif // EIGEN_PARSED_BY_DOXYGEN
+    
+    
+    /** Sets the shift parameter that will be used to adjust the diagonal coefficients during the numerical factorization.
+      *
+      * During the numerical factorization, an offset term is added to the diagonal coefficients:\n
+      * \c d_ii = \a offset + \c d_ii
+      *
+      * The default is \a offset=0.
+      *
+      * \returns a reference to \c *this.
+      */
+    Derived& setShift(const RealScalar& offset)
+    {
+      m_shiftOffset[0] = double(offset);
+      return derived();
+    }
+    
+    /** \returns the determinant of the underlying matrix from the current factorization */
+    Scalar determinant() const
+    {
+      using std::exp;
+      return exp(logDeterminant());
+    }
+
+    /** \returns the log determinant of the underlying matrix from the current factorization */
+    Scalar logDeterminant() const
+    {
+      using std::log;
+      using numext::real;
+      eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for solving, you must first call either compute() or symbolic()/numeric()");
+
+      RealScalar logDet = 0;
+      Scalar *x = static_cast<Scalar*>(m_cholmodFactor->x);
+      if (m_cholmodFactor->is_super)
+      {
+        // Supernodal factorization stored as a packed list of dense column-major blocs,
+        // as described by the following structure:
+
+        // super[k] == index of the first column of the j-th super node
+        StorageIndex *super = static_cast<StorageIndex*>(m_cholmodFactor->super);
+        // pi[k] == offset to the description of row indices
+        StorageIndex *pi = static_cast<StorageIndex*>(m_cholmodFactor->pi);
+        // px[k] == offset to the respective dense block
+        StorageIndex *px = static_cast<StorageIndex*>(m_cholmodFactor->px);
+
+        Index nb_super_nodes = m_cholmodFactor->nsuper;
+        for (Index k=0; k < nb_super_nodes; ++k)
+        {
+          StorageIndex ncols = super[k + 1] - super[k];
+          StorageIndex nrows = pi[k + 1] - pi[k];
+
+          Map<const Array<Scalar,1,Dynamic>, 0, InnerStride<> > sk(x + px[k], ncols, InnerStride<>(nrows+1));
+          logDet += sk.real().log().sum();
+        }
+      }
+      else
+      {
+        // Simplicial factorization stored as standard CSC matrix.
+        StorageIndex *p = static_cast<StorageIndex*>(m_cholmodFactor->p);
+        Index size = m_cholmodFactor->n;
+        for (Index k=0; k<size; ++k)
+          logDet += log(real( x[p[k]] ));
+      }
+      if (m_cholmodFactor->is_ll)
+        logDet *= 2.0;
+      return logDet;
+    };
+
+    template<typename Stream>
+    void dumpMemory(Stream& /*s*/)
+    {}
+    
+  protected:
+    mutable cholmod_common m_cholmod;
+    cholmod_factor* m_cholmodFactor;
+    double m_shiftOffset[2];
+    mutable ComputationInfo m_info;
+    int m_factorizationIsOk;
+    int m_analysisIsOk;
+};
+
+/** \ingroup CholmodSupport_Module
+  * \class CholmodSimplicialLLT
+  * \brief A simplicial direct Cholesky (LLT) factorization and solver based on Cholmod
+  *
+  * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization
+  * using the Cholmod library.
+  * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest.
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
+  * X and B can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  *
+  * \implsparsesolverconcept
+  *
+  * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed.
+  *
+  * \warning Only double precision real and complex scalar types are supported by Cholmod.
+  *
+  * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLLT
+  */
+template<typename _MatrixType, int _UpLo = Lower>
+class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLLT<_MatrixType, _UpLo> >
+{
+    typedef CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLLT> Base;
+    using Base::m_cholmod;
+    
+  public:
+    
+    typedef _MatrixType MatrixType;
+    
+    CholmodSimplicialLLT() : Base() { init(); }
+
+    CholmodSimplicialLLT(const MatrixType& matrix) : Base()
+    {
+      init();
+      this->compute(matrix);
+    }
+
+    ~CholmodSimplicialLLT() {}
+  protected:
+    void init()
+    {
+      m_cholmod.final_asis = 0;
+      m_cholmod.supernodal = CHOLMOD_SIMPLICIAL;
+      m_cholmod.final_ll = 1;
+    }
+};
+
+
+/** \ingroup CholmodSupport_Module
+  * \class CholmodSimplicialLDLT
+  * \brief A simplicial direct Cholesky (LDLT) factorization and solver based on Cholmod
+  *
+  * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization
+  * using the Cholmod library.
+  * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest.
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
+  * X and B can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  *
+  * \implsparsesolverconcept
+  *
+  * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed.
+  *
+  * \warning Only double precision real and complex scalar types are supported by Cholmod.
+  *
+  * \sa \ref TutorialSparseSolverConcept, class CholmodSupernodalLLT, class SimplicialLDLT
+  */
+template<typename _MatrixType, int _UpLo = Lower>
+class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLDLT<_MatrixType, _UpLo> >
+{
+    typedef CholmodBase<_MatrixType, _UpLo, CholmodSimplicialLDLT> Base;
+    using Base::m_cholmod;
+    
+  public:
+    
+    typedef _MatrixType MatrixType;
+    
+    CholmodSimplicialLDLT() : Base() { init(); }
+
+    CholmodSimplicialLDLT(const MatrixType& matrix) : Base()
+    {
+      init();
+      this->compute(matrix);
+    }
+
+    ~CholmodSimplicialLDLT() {}
+  protected:
+    void init()
+    {
+      m_cholmod.final_asis = 1;
+      m_cholmod.supernodal = CHOLMOD_SIMPLICIAL;
+    }
+};
+
+/** \ingroup CholmodSupport_Module
+  * \class CholmodSupernodalLLT
+  * \brief A supernodal Cholesky (LLT) factorization and solver based on Cholmod
+  *
+  * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization
+  * using the Cholmod library.
+  * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM.
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
+  * X and B can be either dense or sparse.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  *
+  * \implsparsesolverconcept
+  *
+  * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed.
+  *
+  * \warning Only double precision real and complex scalar types are supported by Cholmod.
+  *
+  * \sa \ref TutorialSparseSolverConcept
+  */
+template<typename _MatrixType, int _UpLo = Lower>
+class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSupernodalLLT<_MatrixType, _UpLo> >
+{
+    typedef CholmodBase<_MatrixType, _UpLo, CholmodSupernodalLLT> Base;
+    using Base::m_cholmod;
+    
+  public:
+    
+    typedef _MatrixType MatrixType;
+    
+    CholmodSupernodalLLT() : Base() { init(); }
+
+    CholmodSupernodalLLT(const MatrixType& matrix) : Base()
+    {
+      init();
+      this->compute(matrix);
+    }
+
+    ~CholmodSupernodalLLT() {}
+  protected:
+    void init()
+    {
+      m_cholmod.final_asis = 1;
+      m_cholmod.supernodal = CHOLMOD_SUPERNODAL;
+    }
+};
+
+/** \ingroup CholmodSupport_Module
+  * \class CholmodDecomposition
+  * \brief A general Cholesky factorization and solver based on Cholmod
+  *
+  * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization
+  * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
+  * X and B can be either dense or sparse.
+  *
+  * This variant permits to change the underlying Cholesky method at runtime.
+  * On the other hand, it does not provide access to the result of the factorization.
+  * The default is to let Cholmod automatically choose between a simplicial and supernodal factorization.
+  *
+  * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  *               or Upper. Default is Lower.
+  *
+  * \implsparsesolverconcept
+  *
+  * This class supports all kind of SparseMatrix<>: row or column major; upper, lower, or both; compressed or non compressed.
+  *
+  * \warning Only double precision real and complex scalar types are supported by Cholmod.
+  *
+  * \sa \ref TutorialSparseSolverConcept
+  */
+template<typename _MatrixType, int _UpLo = Lower>
+class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecomposition<_MatrixType, _UpLo> >
+{
+    typedef CholmodBase<_MatrixType, _UpLo, CholmodDecomposition> Base;
+    using Base::m_cholmod;
+    
+  public:
+    
+    typedef _MatrixType MatrixType;
+    
+    CholmodDecomposition() : Base() { init(); }
+
+    CholmodDecomposition(const MatrixType& matrix) : Base()
+    {
+      init();
+      this->compute(matrix);
+    }
+
+    ~CholmodDecomposition() {}
+    
+    void setMode(CholmodMode mode)
+    {
+      switch(mode)
+      {
+        case CholmodAuto:
+          m_cholmod.final_asis = 1;
+          m_cholmod.supernodal = CHOLMOD_AUTO;
+          break;
+        case CholmodSimplicialLLt:
+          m_cholmod.final_asis = 0;
+          m_cholmod.supernodal = CHOLMOD_SIMPLICIAL;
+          m_cholmod.final_ll = 1;
+          break;
+        case CholmodSupernodalLLt:
+          m_cholmod.final_asis = 1;
+          m_cholmod.supernodal = CHOLMOD_SUPERNODAL;
+          break;
+        case CholmodLDLt:
+          m_cholmod.final_asis = 1;
+          m_cholmod.supernodal = CHOLMOD_SIMPLICIAL;
+          break;
+        default:
+          break;
+      }
+    }
+  protected:
+    void init()
+    {
+      m_cholmod.final_asis = 1;
+      m_cholmod.supernodal = CHOLMOD_AUTO;
+    }
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CHOLMODSUPPORT_H

examples/ThirdPartyLibs/Eigen/src/Core/ArithmeticSequence.h

@@ -0,0 +1,350 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARITHMETIC_SEQUENCE_H
+#define EIGEN_ARITHMETIC_SEQUENCE_H
+
+namespace Eigen {
+
+namespace internal {
+
+#if (!EIGEN_HAS_CXX11) || !((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
+template<typename T> struct aseq_negate {};
+
+template<> struct aseq_negate<Index> {
+  typedef Index type;
+};
+
+template<int N> struct aseq_negate<FixedInt<N> > {
+  typedef FixedInt<-N> type;
+};
+
+// Compilation error in the following case:
+template<> struct aseq_negate<FixedInt<DynamicIndex> > {};
+
+template<typename FirstType,typename SizeType,typename IncrType,
+         bool FirstIsSymbolic=Symbolic::is_symbolic<FirstType>::value,
+         bool SizeIsSymbolic =Symbolic::is_symbolic<SizeType>::value>
+struct aseq_reverse_first_type {
+  typedef Index type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,true> {
+  typedef Symbolic::AddExpr<FirstType,
+                            Symbolic::ProductExpr<Symbolic::AddExpr<SizeType,Symbolic::ValueExpr<FixedInt<-1> > >,
+                                                  Symbolic::ValueExpr<IncrType> >
+                           > type;
+};
+
+template<typename SizeType,typename IncrType,typename EnableIf = void>
+struct aseq_reverse_first_type_aux {
+  typedef Index type;
+};
+
+template<typename SizeType,typename IncrType>
+struct aseq_reverse_first_type_aux<SizeType,IncrType,typename internal::enable_if<bool((SizeType::value+IncrType::value)|0x1)>::type> {
+  typedef FixedInt<(SizeType::value-1)*IncrType::value> type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,false> {
+  typedef typename aseq_reverse_first_type_aux<SizeType,IncrType>::type Aux;
+  typedef Symbolic::AddExpr<FirstType,Symbolic::ValueExpr<Aux> > type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,false,true> {
+  typedef Symbolic::AddExpr<Symbolic::ProductExpr<Symbolic::AddExpr<SizeType,Symbolic::ValueExpr<FixedInt<-1> > >,
+                                                  Symbolic::ValueExpr<IncrType> >,
+                            Symbolic::ValueExpr<> > type;
+};
+#endif
+
+// Helper to cleanup the type of the increment:
+template<typename T> struct cleanup_seq_incr {
+  typedef typename cleanup_index_type<T,DynamicIndex>::type type;
+};
+
+}
+
+//--------------------------------------------------------------------------------
+// seq(first,last,incr) and seqN(first,size,incr)
+//--------------------------------------------------------------------------------
+
+template<typename FirstType=Index,typename SizeType=Index,typename IncrType=internal::FixedInt<1> >
+class ArithmeticSequence;
+
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                   typename internal::cleanup_index_type<SizeType>::type,
+                   typename internal::cleanup_seq_incr<IncrType>::type >
+seqN(FirstType first, SizeType size, IncrType incr);
+
+/** \class ArithmeticSequence
+  * \ingroup Core_Module
+  *
+  * This class represents an arithmetic progression \f$ a_0, a_1, a_2, ..., a_{n-1}\f$ defined by
+  * its \em first value \f$ a_0 \f$, its \em size (aka length) \em n, and the \em increment (aka stride)
+  * that is equal to \f$ a_{i+1}-a_{i}\f$ for any \em i.
+  *
+  * It is internally used as the return type of the Eigen::seq and Eigen::seqN functions, and as the input arguments
+  * of DenseBase::operator()(const RowIndices&, const ColIndices&), and most of the time this is the
+  * only way it is used.
+  *
+  * \tparam FirstType type of the first element, usually an Index,
+  *                   but internally it can be a symbolic expression
+  * \tparam SizeType type representing the size of the sequence, usually an Index
+  *                  or a compile time integral constant. Internally, it can also be a symbolic expression
+  * \tparam IncrType type of the increment, can be a runtime Index, or a compile time integral constant (default is compile-time 1)
+  *
+  * \sa Eigen::seq, Eigen::seqN, DenseBase::operator()(const RowIndices&, const ColIndices&), class IndexedView
+  */
+template<typename FirstType,typename SizeType,typename IncrType>
+class ArithmeticSequence
+{
+public:
+  ArithmeticSequence(FirstType first, SizeType size) : m_first(first), m_size(size) {}
+  ArithmeticSequence(FirstType first, SizeType size, IncrType incr) : m_first(first), m_size(size), m_incr(incr) {}
+
+  enum {
+    SizeAtCompileTime = internal::get_fixed_value<SizeType>::value,
+    IncrAtCompileTime = internal::get_fixed_value<IncrType,DynamicIndex>::value
+  };
+
+  /** \returns the size, i.e., number of elements, of the sequence */
+  Index size()  const { return m_size; }
+
+  /** \returns the first element \f$ a_0 \f$ in the sequence */
+  Index first()  const { return m_first; }
+
+  /** \returns the value \f$ a_i \f$ at index \a i in the sequence. */
+  Index operator[](Index i) const { return m_first + i * m_incr; }
+
+  const FirstType& firstObject() const { return m_first; }
+  const SizeType&  sizeObject()  const { return m_size; }
+  const IncrType&  incrObject()  const { return m_incr; }
+
+protected:
+  FirstType m_first;
+  SizeType  m_size;
+  IncrType  m_incr;
+
+public:
+
+#if EIGEN_HAS_CXX11 && ((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
+  auto reverse() const -> decltype(Eigen::seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr)) {
+    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
+  }
+#else
+protected:
+  typedef typename internal::aseq_negate<IncrType>::type ReverseIncrType;
+  typedef typename internal::aseq_reverse_first_type<FirstType,SizeType,IncrType>::type ReverseFirstType;
+public:
+  ArithmeticSequence<ReverseFirstType,SizeType,ReverseIncrType>
+  reverse() const {
+    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
+  }
+#endif
+};
+
+/** \returns an ArithmeticSequence starting at \a first, of length \a size, and increment \a incr
+  *
+  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type >
+seqN(FirstType first, SizeType size, IncrType incr)  {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type>(first,size,incr);
+}
+
+/** \returns an ArithmeticSequence starting at \a first, of length \a size, and unit increment
+  *
+  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) */
+template<typename FirstType,typename SizeType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type >
+seqN(FirstType first, SizeType size)  {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type>(first,size);
+}
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+
+/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and with positive (or negative) increment \a incr
+  *
+  * It is essentially an alias to:
+  * \code
+  * seqN(f, (l-f+incr)/incr, incr);
+  * \endcode
+  *
+  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType)
+  */
+template<typename FirstType,typename LastType, typename IncrType>
+auto seq(FirstType f, LastType l, IncrType incr);
+
+/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and unit increment
+  *
+  * It is essentially an alias to:
+  * \code
+  * seqN(f,l-f+1);
+  * \endcode
+  *
+  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType)
+  */
+template<typename FirstType,typename LastType>
+auto seq(FirstType f, LastType l);
+
+#else // EIGEN_PARSED_BY_DOXYGEN
+
+#if EIGEN_HAS_CXX11
+template<typename FirstType,typename LastType>
+auto seq(FirstType f, LastType l) -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                                                   (  typename internal::cleanup_index_type<LastType>::type(l)
+                                                    - typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())))
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              (typename internal::cleanup_index_type<LastType>::type(l)
+               -typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
+}
+
+template<typename FirstType,typename LastType, typename IncrType>
+auto seq(FirstType f, LastType l, IncrType incr)
+  -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                   (   typename internal::cleanup_index_type<LastType>::type(l)
+                     - typename internal::cleanup_index_type<FirstType>::type(f)+typename internal::cleanup_seq_incr<IncrType>::type(incr)
+                   ) / typename internal::cleanup_seq_incr<IncrType>::type(incr),
+                   typename internal::cleanup_seq_incr<IncrType>::type(incr)))
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              ( typename internal::cleanup_index_type<LastType>::type(l)
+               -typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr)) / CleanedIncrType(incr),
+              CleanedIncrType(incr));
+}
+#else
+
+template<typename FirstType,typename LastType>
+typename internal::enable_if<!(Symbolic::is_symbolic<FirstType>::value || Symbolic::is_symbolic<LastType>::value),
+                             ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index> >::type
+seq(FirstType f, LastType l)
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())));
+}
+
+template<typename FirstTypeDerived,typename LastType>
+typename internal::enable_if<!Symbolic::is_symbolic<LastType>::value,
+    ArithmeticSequence<FirstTypeDerived, Symbolic::AddExpr<Symbolic::AddExpr<Symbolic::NegateExpr<FirstTypeDerived>,Symbolic::ValueExpr<> >,
+                                                            Symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
+seq(const Symbolic::BaseExpr<FirstTypeDerived> &f, LastType l)
+{
+  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+fix<1>()));
+}
+
+template<typename FirstType,typename LastTypeDerived>
+typename internal::enable_if<!Symbolic::is_symbolic<FirstType>::value,
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                        Symbolic::AddExpr<Symbolic::AddExpr<LastTypeDerived,Symbolic::ValueExpr<> >,
+                                          Symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
+seq(FirstType f, const Symbolic::BaseExpr<LastTypeDerived> &l)
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),(l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
+}
+
+template<typename FirstTypeDerived,typename LastTypeDerived>
+ArithmeticSequence<FirstTypeDerived,
+                    Symbolic::AddExpr<Symbolic::AddExpr<LastTypeDerived,Symbolic::NegateExpr<FirstTypeDerived> >,Symbolic::ValueExpr<internal::FixedInt<1> > > >
+seq(const Symbolic::BaseExpr<FirstTypeDerived> &f, const Symbolic::BaseExpr<LastTypeDerived> &l)
+{
+  return seqN(f.derived(),(l.derived()-f.derived()+fix<1>()));
+}
+
+
+template<typename FirstType,typename LastType, typename IncrType>
+typename internal::enable_if<!(Symbolic::is_symbolic<FirstType>::value || Symbolic::is_symbolic<LastType>::value),
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index,typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(FirstType f, LastType l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr)), incr);
+}
+
+template<typename FirstTypeDerived,typename LastType, typename IncrType>
+typename internal::enable_if<!Symbolic::is_symbolic<LastType>::value,
+    ArithmeticSequence<FirstTypeDerived,
+                        Symbolic::QuotientExpr<Symbolic::AddExpr<Symbolic::AddExpr<Symbolic::NegateExpr<FirstTypeDerived>,
+                                                                                   Symbolic::ValueExpr<> >,
+                                                                 Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                              Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(const Symbolic::BaseExpr<FirstTypeDerived> &f, LastType l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+
+template<typename FirstType,typename LastTypeDerived, typename IncrType>
+typename internal::enable_if<!Symbolic::is_symbolic<FirstType>::value,
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                        Symbolic::QuotientExpr<Symbolic::AddExpr<Symbolic::AddExpr<LastTypeDerived,Symbolic::ValueExpr<> >,
+                                                                 Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                               Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(FirstType f, const Symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              (l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+
+template<typename FirstTypeDerived,typename LastTypeDerived, typename IncrType>
+ArithmeticSequence<FirstTypeDerived,
+                    Symbolic::QuotientExpr<Symbolic::AddExpr<Symbolic::AddExpr<LastTypeDerived,
+                                                                               Symbolic::NegateExpr<FirstTypeDerived> >,
+                                                             Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                          Symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                    typename internal::cleanup_seq_incr<IncrType>::type>
+seq(const Symbolic::BaseExpr<FirstTypeDerived> &f, const Symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(f.derived(),(l.derived()-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+#endif
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+namespace internal {
+
+// Convert a symbolic span into a usable one (i.e., remove last/end "keywords")
+template<typename T>
+struct make_size_type {
+  typedef typename internal::conditional<Symbolic::is_symbolic<T>::value, Index, T>::type type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType,int XprSize>
+struct IndexedViewCompatibleType<ArithmeticSequence<FirstType,SizeType,IncrType>, XprSize> {
+  typedef ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType> type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>
+makeIndexedViewCompatible(const ArithmeticSequence<FirstType,SizeType,IncrType>& ids, Index size,SpecializedType) {
+  return ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>(
+            eval_expr_given_size(ids.firstObject(),size),eval_expr_given_size(ids.sizeObject(),size),ids.incrObject());
+}
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct get_compile_time_incr<ArithmeticSequence<FirstType,SizeType,IncrType> > {
+  enum { value = get_fixed_value<IncrType,DynamicIndex>::value };
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARITHMETIC_SEQUENCE_H

examples/ThirdPartyLibs/Eigen/src/Core/Array.h

@@ -0,0 +1,331 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARRAY_H
+#define EIGEN_ARRAY_H
+
+namespace Eigen {
+
+namespace internal {
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+struct traits<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > : traits<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
+{
+  typedef ArrayXpr XprKind;
+  typedef ArrayBase<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > XprBase;
+};
+}
+
+/** \class Array
+  * \ingroup Core_Module
+  *
+  * \brief General-purpose arrays with easy API for coefficient-wise operations
+  *
+  * The %Array class is very similar to the Matrix class. It provides
+  * general-purpose one- and two-dimensional arrays. The difference between the
+  * %Array and the %Matrix class is primarily in the API: the API for the
+  * %Array class provides easy access to coefficient-wise operations, while the
+  * API for the %Matrix class provides easy access to linear-algebra
+  * operations.
+  *
+  * See documentation of class Matrix for detailed information on the template parameters
+  * storage layout.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAY_PLUGIN.
+  *
+  * \sa \blank \ref TutorialArrayClass, \ref TopicClassHierarchy
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+class Array
+  : public PlainObjectBase<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
+{
+  public:
+
+    typedef PlainObjectBase<Array> Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(Array)
+
+    enum { Options = _Options };
+    typedef typename Base::PlainObject PlainObject;
+
+  protected:
+    template <typename Derived, typename OtherDerived, bool IsVector>
+    friend struct internal::conservative_resize_like_impl;
+
+    using Base::m_storage;
+
+  public:
+
+    using Base::base;
+    using Base::coeff;
+    using Base::coeffRef;
+
+    /**
+      * The usage of
+      *   using Base::operator=;
+      * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
+      * the usage of 'using'. This should be done only for operator=.
+      */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array& operator=(const EigenBase<OtherDerived> &other)
+    {
+      return Base::operator=(other);
+    }
+
+    /** Set all the entries to \a value.
+      * \sa DenseBase::setConstant(), DenseBase::fill()
+      */
+    /* This overload is needed because the usage of
+      *   using Base::operator=;
+      * fails on MSVC. Since the code below is working with GCC and MSVC, we skipped
+      * the usage of 'using'. This should be done only for operator=.
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array& operator=(const Scalar &value)
+    {
+      Base::setConstant(value);
+      return *this;
+    }
+
+    /** Copies the value of the expression \a other into \c *this with automatic resizing.
+      *
+      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+      * it will be initialized.
+      *
+      * Note that copying a row-vector into a vector (and conversely) is allowed.
+      * The resizing, if any, is then done in the appropriate way so that row-vectors
+      * remain row-vectors and vectors remain vectors.
+      */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array& operator=(const DenseBase<OtherDerived>& other)
+    {
+      return Base::_set(other);
+    }
+
+    /** This is a special case of the templated operator=. Its purpose is to
+      * prevent a default operator= from hiding the templated operator=.
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array& operator=(const Array& other)
+    {
+      return Base::_set(other);
+    }
+    
+    /** Default constructor.
+      *
+      * For fixed-size matrices, does nothing.
+      *
+      * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
+      * is called a null matrix. This constructor is the unique way to create null matrices: resizing
+      * a matrix to 0 is not supported.
+      *
+      * \sa resize(Index,Index)
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array() : Base()
+    {
+      Base::_check_template_params();
+      EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    // FIXME is it still needed ??
+    /** \internal */
+    EIGEN_DEVICE_FUNC
+    Array(internal::constructor_without_unaligned_array_assert)
+      : Base(internal::constructor_without_unaligned_array_assert())
+    {
+      Base::_check_template_params();
+      EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
+    }
+#endif
+
+#if EIGEN_HAS_RVALUE_REFERENCES
+    EIGEN_DEVICE_FUNC
+    Array(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
+      : Base(std::move(other))
+    {
+      Base::_check_template_params();
+      if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
+        Base::_set_noalias(other);
+    }
+    EIGEN_DEVICE_FUNC
+    Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
+    {
+      other.swap(*this);
+      return *this;
+    }
+#endif
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename T>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE explicit Array(const T& x)
+    {
+      Base::_check_template_params();
+      Base::template _init1<T>(x);
+    }
+
+    template<typename T0, typename T1>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const T0& val0, const T1& val1)
+    {
+      Base::_check_template_params();
+      this->template _init2<T0,T1>(val0, val1);
+    }
+    #else
+    /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */
+    EIGEN_DEVICE_FUNC explicit Array(const Scalar *data);
+    /** Constructs a vector or row-vector with given dimension. \only_for_vectors
+      *
+      * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,
+      * it is redundant to pass the dimension here, so it makes more sense to use the default
+      * constructor Array() instead.
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE explicit Array(Index dim);
+    /** constructs an initialized 1x1 Array with the given coefficient */
+    Array(const Scalar& value);
+    /** constructs an uninitialized array with \a rows rows and \a cols columns.
+      *
+      * This is useful for dynamic-size arrays. For fixed-size arrays,
+      * it is redundant to pass these parameters, so one should use the default constructor
+      * Array() instead. */
+    Array(Index rows, Index cols);
+    /** constructs an initialized 2D vector with given coefficients */
+    Array(const Scalar& val0, const Scalar& val1);
+    #endif
+
+    /** constructs an initialized 3D vector with given coefficients */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2)
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 3)
+      m_storage.data()[0] = val0;
+      m_storage.data()[1] = val1;
+      m_storage.data()[2] = val2;
+    }
+    /** constructs an initialized 4D vector with given coefficients */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3)
+    {
+      Base::_check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Array, 4)
+      m_storage.data()[0] = val0;
+      m_storage.data()[1] = val1;
+      m_storage.data()[2] = val2;
+      m_storage.data()[3] = val3;
+    }
+
+    /** Copy constructor */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const Array& other)
+            : Base(other)
+    { }
+
+  private:
+    struct PrivateType {};
+  public:
+
+    /** \sa MatrixBase::operator=(const EigenBase<OtherDerived>&) */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const EigenBase<OtherDerived> &other,
+                              typename internal::enable_if<internal::is_convertible<typename OtherDerived::Scalar,Scalar>::value,
+                                                           PrivateType>::type = PrivateType())
+      : Base(other.derived())
+    { }
+
+    EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; }
+    EIGEN_DEVICE_FUNC inline Index outerStride() const { return this->innerSize(); }
+
+    #ifdef EIGEN_ARRAY_PLUGIN
+    #include EIGEN_ARRAY_PLUGIN
+    #endif
+
+  private:
+
+    template<typename MatrixType, typename OtherDerived, bool SwapPointers>
+    friend struct internal::matrix_swap_impl;
+};
+
+/** \defgroup arraytypedefs Global array typedefs
+  * \ingroup Core_Module
+  *
+  * Eigen defines several typedef shortcuts for most common 1D and 2D array types.
+  *
+  * The general patterns are the following:
+  *
+  * \c ArrayRowsColsType where \c Rows and \c Cols can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
+  * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
+  * for complex double.
+  *
+  * For example, \c Array33d is a fixed-size 3x3 array type of doubles, and \c ArrayXXf is a dynamic-size matrix of floats.
+  *
+  * There are also \c ArraySizeType which are self-explanatory. For example, \c Array4cf is
+  * a fixed-size 1D array of 4 complex floats.
+  *
+  * \sa class Array
+  */
+
+#define EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)   \
+/** \ingroup arraytypedefs */                                    \
+typedef Array<Type, Size, Size> Array##SizeSuffix##SizeSuffix##TypeSuffix;  \
+/** \ingroup arraytypedefs */                                    \
+typedef Array<Type, Size, 1>    Array##SizeSuffix##TypeSuffix;
+
+#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, Size)         \
+/** \ingroup arraytypedefs */                                    \
+typedef Array<Type, Size, Dynamic> Array##Size##X##TypeSuffix;  \
+/** \ingroup arraytypedefs */                                    \
+typedef Array<Type, Dynamic, Size> Array##X##Size##TypeSuffix;
+
+#define EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
+EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 2, 2) \
+EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 3, 3) \
+EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, 4, 4) \
+EIGEN_MAKE_ARRAY_TYPEDEFS(Type, TypeSuffix, Dynamic, X) \
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Type, TypeSuffix, 4)
+
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(int,                  i)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(float,                f)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(double,               d)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex<float>,  cf)
+EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
+
+#undef EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES
+#undef EIGEN_MAKE_ARRAY_TYPEDEFS
+
+#undef EIGEN_MAKE_ARRAY_TYPEDEFS_LARGE
+
+#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
+using Eigen::Matrix##SizeSuffix##TypeSuffix; \
+using Eigen::Vector##SizeSuffix##TypeSuffix; \
+using Eigen::RowVector##SizeSuffix##TypeSuffix;
+
+#define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(TypeSuffix) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
+
+#define EIGEN_USING_ARRAY_TYPEDEFS \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(i) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(f) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(d) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cf) \
+EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE(cd)
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARRAY_H

examples/ThirdPartyLibs/Eigen/src/Core/ArrayBase.h

@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARRAYBASE_H
+#define EIGEN_ARRAYBASE_H
+
+namespace Eigen { 
+
+template<typename ExpressionType> class MatrixWrapper;
+
+/** \class ArrayBase
+  * \ingroup Core_Module
+  *
+  * \brief Base class for all 1D and 2D array, and related expressions
+  *
+  * An array is similar to a dense vector or matrix. While matrices are mathematical
+  * objects with well defined linear algebra operators, an array is just a collection
+  * of scalar values arranged in a one or two dimensionnal fashion. As the main consequence,
+  * all operations applied to an array are performed coefficient wise. Furthermore,
+  * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient
+  * constructors allowing to easily write generic code working for both scalar values
+  * and arrays.
+  *
+  * This class is the base that is inherited by all array expression types.
+  *
+  * \tparam Derived is the derived type, e.g., an array or an expression type.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN.
+  *
+  * \sa class MatrixBase, \ref TopicClassHierarchy
+  */
+template<typename Derived> class ArrayBase
+  : public DenseBase<Derived>
+{
+  public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** The base class for a given storage type. */
+    typedef ArrayBase StorageBaseType;
+
+    typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
+
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    typedef DenseBase<Derived> Base;
+    using Base::RowsAtCompileTime;
+    using Base::ColsAtCompileTime;
+    using Base::SizeAtCompileTime;
+    using Base::MaxRowsAtCompileTime;
+    using Base::MaxColsAtCompileTime;
+    using Base::MaxSizeAtCompileTime;
+    using Base::IsVectorAtCompileTime;
+    using Base::Flags;
+    
+    using Base::derived;
+    using Base::const_cast_derived;
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::coeff;
+    using Base::coeffRef;
+    using Base::lazyAssign;
+    using Base::operator-;
+    using Base::operator=;
+    using Base::operator+=;
+    using Base::operator-=;
+    using Base::operator*=;
+    using Base::operator/=;
+
+    typedef typename Base::CoeffReturnType CoeffReturnType;
+
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typedef typename Base::PlainObject PlainObject;
+
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#   include "../plugins/MatrixCwiseUnaryOps.h"
+#   include "../plugins/ArrayCwiseUnaryOps.h"
+#   include "../plugins/CommonCwiseBinaryOps.h"
+#   include "../plugins/MatrixCwiseBinaryOps.h"
+#   include "../plugins/ArrayCwiseBinaryOps.h"
+#   ifdef EIGEN_ARRAYBASE_PLUGIN
+#     include EIGEN_ARRAYBASE_PLUGIN
+#   endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_UNARY_ADDONS
+
+    /** Special case of the template operator=, in order to prevent the compiler
+      * from generating a default operator= (issue hit with g++ 4.1)
+      */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const ArrayBase& other)
+    {
+      internal::call_assignment(derived(), other.derived());
+      return derived();
+    }
+    
+    /** Set all the entries to \a value.
+      * \sa DenseBase::setConstant(), DenseBase::fill() */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const Scalar &value)
+    { Base::setConstant(value); return derived(); }
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator+=(const Scalar& scalar);
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator-=(const Scalar& scalar);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator+=(const ArrayBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator-=(const ArrayBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator*=(const ArrayBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator/=(const ArrayBase<OtherDerived>& other);
+
+  public:
+    EIGEN_DEVICE_FUNC
+    ArrayBase<Derived>& array() { return *this; }
+    EIGEN_DEVICE_FUNC
+    const ArrayBase<Derived>& array() const { return *this; }
+
+    /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array
+      * \sa MatrixBase::array() */
+    EIGEN_DEVICE_FUNC
+    MatrixWrapper<Derived> matrix() { return MatrixWrapper<Derived>(derived()); }
+    EIGEN_DEVICE_FUNC
+    const MatrixWrapper<const Derived> matrix() const { return MatrixWrapper<const Derived>(derived()); }
+
+//     template<typename Dest>
+//     inline void evalTo(Dest& dst) const { dst = matrix(); }
+
+  protected:
+    EIGEN_DEVICE_FUNC
+    ArrayBase() : Base() {}
+
+  private:
+    explicit ArrayBase(Index);
+    ArrayBase(Index,Index);
+    template<typename OtherDerived> explicit ArrayBase(const ArrayBase<OtherDerived>&);
+  protected:
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator+=(const MatrixBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator-=(const MatrixBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+};
+
+/** replaces \c *this by \c *this - \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator-=(const ArrayBase<OtherDerived> &other)
+{
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this + \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator+=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this * \a other coefficient wise.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator*=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::mul_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this / \a other coefficient wise.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator/=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::div_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARRAYBASE_H

examples/ThirdPartyLibs/Eigen/src/Core/ArrayWrapper.h

@@ -0,0 +1,209 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARRAYWRAPPER_H
+#define EIGEN_ARRAYWRAPPER_H
+
+namespace Eigen { 
+
+/** \class ArrayWrapper
+  * \ingroup Core_Module
+  *
+  * \brief Expression of a mathematical vector or matrix as an array object
+  *
+  * This class is the return type of MatrixBase::array(), and most of the time
+  * this is the only way it is use.
+  *
+  * \sa MatrixBase::array(), class MatrixWrapper
+  */
+
+namespace internal {
+template<typename ExpressionType>
+struct traits<ArrayWrapper<ExpressionType> >
+  : public traits<typename remove_all<typename ExpressionType::Nested>::type >
+{
+  typedef ArrayXpr XprKind;
+  // Let's remove NestByRefBit
+  enum {
+    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    LvalueBitFlag = is_lvalue<ExpressionType>::value ? LvalueBit : 0,
+    Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag
+  };
+};
+}
+
+template<typename ExpressionType>
+class ArrayWrapper : public ArrayBase<ArrayWrapper<ExpressionType> >
+{
+  public:
+    typedef ArrayBase<ArrayWrapper> Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(ArrayWrapper)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ArrayWrapper)
+    typedef typename internal::remove_all<ExpressionType>::type NestedExpression;
+
+    typedef typename internal::conditional<
+                       internal::is_lvalue<ExpressionType>::value,
+                       Scalar,
+                       const Scalar
+                     >::type ScalarWithConstIfNotLvalue;
+
+    typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
+
+    using Base::coeffRef;
+
+    EIGEN_DEVICE_FUNC
+    explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {}
+
+    EIGEN_DEVICE_FUNC
+    inline Index rows() const { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC
+    inline Index cols() const { return m_expression.cols(); }
+    EIGEN_DEVICE_FUNC
+    inline Index outerStride() const { return m_expression.outerStride(); }
+    EIGEN_DEVICE_FUNC
+    inline Index innerStride() const { return m_expression.innerStride(); }
+
+    EIGEN_DEVICE_FUNC
+    inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
+    EIGEN_DEVICE_FUNC
+    inline const Scalar* data() const { return m_expression.data(); }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    {
+      return m_expression.coeffRef(rowId, colId);
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index index) const
+    {
+      return m_expression.coeffRef(index);
+    }
+
+    template<typename Dest>
+    EIGEN_DEVICE_FUNC
+    inline void evalTo(Dest& dst) const { dst = m_expression; }
+
+    const typename internal::remove_all<NestedExpressionType>::type& 
+    EIGEN_DEVICE_FUNC
+    nestedExpression() const 
+    {
+      return m_expression;
+    }
+
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index)  */
+    EIGEN_DEVICE_FUNC
+    void resize(Index newSize) { m_expression.resize(newSize); }
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index,Index)*/
+    EIGEN_DEVICE_FUNC
+    void resize(Index rows, Index cols) { m_expression.resize(rows,cols); }
+
+  protected:
+    NestedExpressionType m_expression;
+};
+
+/** \class MatrixWrapper
+  * \ingroup Core_Module
+  *
+  * \brief Expression of an array as a mathematical vector or matrix
+  *
+  * This class is the return type of ArrayBase::matrix(), and most of the time
+  * this is the only way it is use.
+  *
+  * \sa MatrixBase::matrix(), class ArrayWrapper
+  */
+
+namespace internal {
+template<typename ExpressionType>
+struct traits<MatrixWrapper<ExpressionType> >
+ : public traits<typename remove_all<typename ExpressionType::Nested>::type >
+{
+  typedef MatrixXpr XprKind;
+  // Let's remove NestByRefBit
+  enum {
+    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    LvalueBitFlag = is_lvalue<ExpressionType>::value ? LvalueBit : 0,
+    Flags = (Flags0 & ~(NestByRefBit | LvalueBit)) | LvalueBitFlag
+  };
+};
+}
+
+template<typename ExpressionType>
+class MatrixWrapper : public MatrixBase<MatrixWrapper<ExpressionType> >
+{
+  public:
+    typedef MatrixBase<MatrixWrapper<ExpressionType> > Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(MatrixWrapper)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MatrixWrapper)
+    typedef typename internal::remove_all<ExpressionType>::type NestedExpression;
+
+    typedef typename internal::conditional<
+                       internal::is_lvalue<ExpressionType>::value,
+                       Scalar,
+                       const Scalar
+                     >::type ScalarWithConstIfNotLvalue;
+
+    typedef typename internal::ref_selector<ExpressionType>::non_const_type NestedExpressionType;
+
+    using Base::coeffRef;
+
+    EIGEN_DEVICE_FUNC
+    explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {}
+
+    EIGEN_DEVICE_FUNC
+    inline Index rows() const { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC
+    inline Index cols() const { return m_expression.cols(); }
+    EIGEN_DEVICE_FUNC
+    inline Index outerStride() const { return m_expression.outerStride(); }
+    EIGEN_DEVICE_FUNC
+    inline Index innerStride() const { return m_expression.innerStride(); }
+
+    EIGEN_DEVICE_FUNC
+    inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
+    EIGEN_DEVICE_FUNC
+    inline const Scalar* data() const { return m_expression.data(); }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    {
+      return m_expression.derived().coeffRef(rowId, colId);
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index index) const
+    {
+      return m_expression.coeffRef(index);
+    }
+
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<NestedExpressionType>::type& 
+    nestedExpression() const 
+    {
+      return m_expression;
+    }
+
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index)  */
+    EIGEN_DEVICE_FUNC
+    void resize(Index newSize) { m_expression.resize(newSize); }
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index,Index)*/
+    EIGEN_DEVICE_FUNC
+    void resize(Index rows, Index cols) { m_expression.resize(rows,cols); }
+
+  protected:
+    NestedExpressionType m_expression;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARRAYWRAPPER_H

examples/ThirdPartyLibs/Eigen/src/Core/Assign.h

@@ -0,0 +1,90 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2007 Michael Olbrich <michael.olbrich@gmx.net>
+// Copyright (C) 2006-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ASSIGN_H
+#define EIGEN_ASSIGN_H
+
+namespace Eigen {
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>
+  ::lazyAssign(const DenseBase<OtherDerived>& other)
+{
+  enum{
+    SameType = internal::is_same<typename Derived::Scalar,typename OtherDerived::Scalar>::value
+  };
+
+  EIGEN_STATIC_ASSERT_LVALUE(Derived)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT(SameType,YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  eigen_assert(rows() == other.rows() && cols() == other.cols());
+  internal::call_assignment_no_alias(derived(),other.derived());
+  
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase<OtherDerived>& other)
+{
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator=(const DenseBase& other)
+{
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const MatrixBase& other)
+{
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const DenseBase<OtherDerived>& other)
+{
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+template <typename OtherDerived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const EigenBase<OtherDerived>& other)
+{
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
+{
+  other.derived().evalTo(derived());
+  return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_ASSIGN_H

examples/ThirdPartyLibs/Eigen/src/Core/AssignEvaluator.h

@@ -0,0 +1,935 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2011-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2011-2012 Jitse Niesen <jitse@maths.leeds.ac.uk>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ASSIGN_EVALUATOR_H
+#define EIGEN_ASSIGN_EVALUATOR_H
+
+namespace Eigen {
+
+// This implementation is based on Assign.h
+
+namespace internal {
+  
+/***************************************************************************
+* Part 1 : the logic deciding a strategy for traversal and unrolling       *
+***************************************************************************/
+
+// copy_using_evaluator_traits is based on assign_traits
+
+template <typename DstEvaluator, typename SrcEvaluator, typename AssignFunc>
+struct copy_using_evaluator_traits
+{
+  typedef typename DstEvaluator::XprType Dst;
+  typedef typename Dst::Scalar DstScalar;
+  
+  enum {
+    DstFlags = DstEvaluator::Flags,
+    SrcFlags = SrcEvaluator::Flags
+  };
+  
+public:
+  enum {
+    DstAlignment = DstEvaluator::Alignment,
+    SrcAlignment = SrcEvaluator::Alignment,
+    DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit,
+    JointAlignment = EIGEN_PLAIN_ENUM_MIN(DstAlignment,SrcAlignment)
+  };
+
+private:
+  enum {
+    InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime)
+              : int(DstFlags)&RowMajorBit ? int(Dst::ColsAtCompileTime)
+              : int(Dst::RowsAtCompileTime),
+    InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime)
+              : int(DstFlags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime)
+              : int(Dst::MaxRowsAtCompileTime),
+    OuterStride = int(outer_stride_at_compile_time<Dst>::ret),
+    MaxSizeAtCompileTime = Dst::SizeAtCompileTime
+  };
+
+  // TODO distinguish between linear traversal and inner-traversals
+  typedef typename find_best_packet<DstScalar,Dst::SizeAtCompileTime>::type LinearPacketType;
+  typedef typename find_best_packet<DstScalar,InnerSize>::type InnerPacketType;
+
+  enum {
+    LinearPacketSize = unpacket_traits<LinearPacketType>::size,
+    InnerPacketSize = unpacket_traits<InnerPacketType>::size
+  };
+
+public:
+  enum {
+    LinearRequiredAlignment = unpacket_traits<LinearPacketType>::alignment,
+    InnerRequiredAlignment = unpacket_traits<InnerPacketType>::alignment
+  };
+
+private:
+  enum {
+    DstIsRowMajor = DstFlags&RowMajorBit,
+    SrcIsRowMajor = SrcFlags&RowMajorBit,
+    StorageOrdersAgree = (int(DstIsRowMajor) == int(SrcIsRowMajor)),
+    MightVectorize = bool(StorageOrdersAgree)
+                  && (int(DstFlags) & int(SrcFlags) & ActualPacketAccessBit)
+                  && bool(functor_traits<AssignFunc>::PacketAccess),
+    MayInnerVectorize  = MightVectorize
+                       && int(InnerSize)!=Dynamic && int(InnerSize)%int(InnerPacketSize)==0
+                       && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0
+                       && (EIGEN_UNALIGNED_VECTORIZE  || int(JointAlignment)>=int(InnerRequiredAlignment)),
+    MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit),
+    MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess)
+                       && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic),
+      /* If the destination isn't aligned, we have to do runtime checks and we don't unroll,
+         so it's only good for large enough sizes. */
+    MaySliceVectorize  = bool(MightVectorize) && bool(DstHasDirectAccess)
+                       && (int(InnerMaxSize)==Dynamic || int(InnerMaxSize)>=(EIGEN_UNALIGNED_VECTORIZE?InnerPacketSize:(3*InnerPacketSize)))
+      /* slice vectorization can be slow, so we only want it if the slices are big, which is
+         indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block
+         in a fixed-size matrix
+         However, with EIGEN_UNALIGNED_VECTORIZE and unrolling, slice vectorization is still worth it */
+  };
+
+public:
+  enum {
+    Traversal = int(MayLinearVectorize) && (LinearPacketSize>InnerPacketSize) ? int(LinearVectorizedTraversal)
+              : int(MayInnerVectorize)   ? int(InnerVectorizedTraversal)
+              : int(MayLinearVectorize)  ? int(LinearVectorizedTraversal)
+              : int(MaySliceVectorize)   ? int(SliceVectorizedTraversal)
+              : int(MayLinearize)        ? int(LinearTraversal)
+                                         : int(DefaultTraversal),
+    Vectorized = int(Traversal) == InnerVectorizedTraversal
+              || int(Traversal) == LinearVectorizedTraversal
+              || int(Traversal) == SliceVectorizedTraversal
+  };
+
+  typedef typename conditional<int(Traversal)==LinearVectorizedTraversal, LinearPacketType, InnerPacketType>::type PacketType;
+
+private:
+  enum {
+    ActualPacketSize    = int(Traversal)==LinearVectorizedTraversal ? LinearPacketSize
+                        : Vectorized ? InnerPacketSize
+                        : 1,
+    UnrollingLimit      = EIGEN_UNROLLING_LIMIT * ActualPacketSize,
+    MayUnrollCompletely = int(Dst::SizeAtCompileTime) != Dynamic
+                       && int(Dst::SizeAtCompileTime) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit),
+    MayUnrollInner      = int(InnerSize) != Dynamic
+                       && int(InnerSize) * (int(DstEvaluator::CoeffReadCost)+int(SrcEvaluator::CoeffReadCost)) <= int(UnrollingLimit)
+  };
+
+public:
+  enum {
+    Unrolling = (int(Traversal) == int(InnerVectorizedTraversal) || int(Traversal) == int(DefaultTraversal))
+                ? (
+                    int(MayUnrollCompletely) ? int(CompleteUnrolling)
+                  : int(MayUnrollInner)      ? int(InnerUnrolling)
+                                             : int(NoUnrolling)
+                  )
+              : int(Traversal) == int(LinearVectorizedTraversal)
+                ? ( bool(MayUnrollCompletely) && ( EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)))
+                          ? int(CompleteUnrolling)
+                          : int(NoUnrolling) )
+              : int(Traversal) == int(LinearTraversal)
+                ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling) 
+                                              : int(NoUnrolling) )
+#if EIGEN_UNALIGNED_VECTORIZE
+              : int(Traversal) == int(SliceVectorizedTraversal)
+                ? ( bool(MayUnrollInner) ? int(InnerUnrolling)
+                                         : int(NoUnrolling) )
+#endif
+              : int(NoUnrolling)
+  };
+
+#ifdef EIGEN_DEBUG_ASSIGN
+  static void debug()
+  {
+    std::cerr << "DstXpr: " << typeid(typename DstEvaluator::XprType).name() << std::endl;
+    std::cerr << "SrcXpr: " << typeid(typename SrcEvaluator::XprType).name() << std::endl;
+    std::cerr.setf(std::ios::hex, std::ios::basefield);
+    std::cerr << "DstFlags" << " = " << DstFlags << " (" << demangle_flags(DstFlags) << " )" << std::endl;
+    std::cerr << "SrcFlags" << " = " << SrcFlags << " (" << demangle_flags(SrcFlags) << " )" << std::endl;
+    std::cerr.unsetf(std::ios::hex);
+    EIGEN_DEBUG_VAR(DstAlignment)
+    EIGEN_DEBUG_VAR(SrcAlignment)
+    EIGEN_DEBUG_VAR(LinearRequiredAlignment)
+    EIGEN_DEBUG_VAR(InnerRequiredAlignment)
+    EIGEN_DEBUG_VAR(JointAlignment)
+    EIGEN_DEBUG_VAR(InnerSize)
+    EIGEN_DEBUG_VAR(InnerMaxSize)
+    EIGEN_DEBUG_VAR(LinearPacketSize)
+    EIGEN_DEBUG_VAR(InnerPacketSize)
+    EIGEN_DEBUG_VAR(ActualPacketSize)
+    EIGEN_DEBUG_VAR(StorageOrdersAgree)
+    EIGEN_DEBUG_VAR(MightVectorize)
+    EIGEN_DEBUG_VAR(MayLinearize)
+    EIGEN_DEBUG_VAR(MayInnerVectorize)
+    EIGEN_DEBUG_VAR(MayLinearVectorize)
+    EIGEN_DEBUG_VAR(MaySliceVectorize)
+    std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
+    EIGEN_DEBUG_VAR(SrcEvaluator::CoeffReadCost)
+    EIGEN_DEBUG_VAR(UnrollingLimit)
+    EIGEN_DEBUG_VAR(MayUnrollCompletely)
+    EIGEN_DEBUG_VAR(MayUnrollInner)
+    std::cerr << "Unrolling" << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
+    std::cerr << std::endl;
+  }
+#endif
+};
+
+/***************************************************************************
+* Part 2 : meta-unrollers
+***************************************************************************/
+
+/************************
+*** Default traversal ***
+************************/
+
+template<typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling
+{
+  // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
+  typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
+  typedef typename DstEvaluatorType::XprType DstXprType;
+  
+  enum {
+    outer = Index / DstXprType::InnerSizeAtCompileTime,
+    inner = Index % DstXprType::InnerSizeAtCompileTime
+  };
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    kernel.assignCoeffByOuterInner(outer, inner);
+    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Index+1, Stop>::run(kernel);
+  }
+};
+
+template<typename Kernel, int Stop>
+struct copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, Stop, Stop>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+};
+
+template<typename Kernel, int Index_, int Stop>
+struct copy_using_evaluator_DefaultTraversal_InnerUnrolling
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer)
+  {
+    kernel.assignCoeffByOuterInner(outer, Index_);
+    copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Index_+1, Stop>::run(kernel, outer);
+  }
+};
+
+template<typename Kernel, int Stop>
+struct copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, Stop, Stop>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index) { }
+};
+
+/***********************
+*** Linear traversal ***
+***********************/
+
+template<typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_LinearTraversal_CompleteUnrolling
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel)
+  {
+    kernel.assignCoeff(Index);
+    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Index+1, Stop>::run(kernel);
+  }
+};
+
+template<typename Kernel, int Stop>
+struct copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, Stop, Stop>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+};
+
+/**************************
+*** Inner vectorization ***
+**************************/
+
+template<typename Kernel, int Index, int Stop>
+struct copy_using_evaluator_innervec_CompleteUnrolling
+{
+  // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
+  typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
+  typedef typename DstEvaluatorType::XprType DstXprType;
+  typedef typename Kernel::PacketType PacketType;
+  
+  enum {
+    outer = Index / DstXprType::InnerSizeAtCompileTime,
+    inner = Index % DstXprType::InnerSizeAtCompileTime,
+    SrcAlignment = Kernel::AssignmentTraits::SrcAlignment,
+    DstAlignment = Kernel::AssignmentTraits::DstAlignment
+  };
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
+    enum { NextIndex = Index + unpacket_traits<PacketType>::size };
+    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
+  }
+};
+
+template<typename Kernel, int Stop>
+struct copy_using_evaluator_innervec_CompleteUnrolling<Kernel, Stop, Stop>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&) { }
+};
+
+template<typename Kernel, int Index_, int Stop, int SrcAlignment, int DstAlignment>
+struct copy_using_evaluator_innervec_InnerUnrolling
+{
+  typedef typename Kernel::PacketType PacketType;
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel, Index outer)
+  {
+    kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, Index_);
+    enum { NextIndex = Index_ + unpacket_traits<PacketType>::size };
+    copy_using_evaluator_innervec_InnerUnrolling<Kernel, NextIndex, Stop, SrcAlignment, DstAlignment>::run(kernel, outer);
+  }
+};
+
+template<typename Kernel, int Stop, int SrcAlignment, int DstAlignment>
+struct copy_using_evaluator_innervec_InnerUnrolling<Kernel, Stop, Stop, SrcAlignment, DstAlignment>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &, Index) { }
+};
+
+/***************************************************************************
+* Part 3 : implementation of all cases
+***************************************************************************/
+
+// dense_assignment_loop is based on assign_impl
+
+template<typename Kernel,
+         int Traversal = Kernel::AssignmentTraits::Traversal,
+         int Unrolling = Kernel::AssignmentTraits::Unrolling>
+struct dense_assignment_loop;
+
+/************************
+*** Default traversal ***
+************************/
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>
+{
+  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel &kernel)
+  {
+    for(Index outer = 0; outer < kernel.outerSize(); ++outer) {
+      for(Index inner = 0; inner < kernel.innerSize(); ++inner) {
+        kernel.assignCoeffByOuterInner(outer, inner);
+      }
+    }
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, CompleteUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, DefaultTraversal, InnerUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+
+    const Index outerSize = kernel.outerSize();
+    for(Index outer = 0; outer < outerSize; ++outer)
+      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime>::run(kernel, outer);
+  }
+};
+
+/***************************
+*** Linear vectorization ***
+***************************/
+
+
+// The goal of unaligned_dense_assignment_loop is simply to factorize the handling
+// of the non vectorizable beginning and ending parts
+
+template <bool IsAligned = false>
+struct unaligned_dense_assignment_loop
+{
+  // if IsAligned = true, then do nothing
+  template <typename Kernel>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel&, Index, Index) {}
+};
+
+template <>
+struct unaligned_dense_assignment_loop<false>
+{
+  // MSVC must not inline this functions. If it does, it fails to optimize the
+  // packet access path.
+  // FIXME check which version exhibits this issue
+#if EIGEN_COMP_MSVC
+  template <typename Kernel>
+  static EIGEN_DONT_INLINE void run(Kernel &kernel,
+                                    Index start,
+                                    Index end)
+#else
+  template <typename Kernel>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel,
+                                      Index start,
+                                      Index end)
+#endif
+  {
+    for (Index index = start; index < end; ++index)
+      kernel.assignCoeff(index);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    const Index size = kernel.size();
+    typedef typename Kernel::Scalar Scalar;
+    typedef typename Kernel::PacketType PacketType;
+    enum {
+      requestedAlignment = Kernel::AssignmentTraits::LinearRequiredAlignment,
+      packetSize = unpacket_traits<PacketType>::size,
+      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment),
+      dstAlignment = packet_traits<Scalar>::AlignedOnScalar ? int(requestedAlignment)
+                                                            : int(Kernel::AssignmentTraits::DstAlignment),
+      srcAlignment = Kernel::AssignmentTraits::JointAlignment
+    };
+    const Index alignedStart = dstIsAligned ? 0 : internal::first_aligned<requestedAlignment>(kernel.dstDataPtr(), size);
+    const Index alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize;
+
+    unaligned_dense_assignment_loop<dstIsAligned!=0>::run(kernel, 0, alignedStart);
+
+    for(Index index = alignedStart; index < alignedEnd; index += packetSize)
+      kernel.template assignPacket<dstAlignment, srcAlignment, PacketType>(index);
+
+    unaligned_dense_assignment_loop<>::run(kernel, alignedEnd, size);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    typedef typename Kernel::PacketType PacketType;
+    
+    enum { size = DstXprType::SizeAtCompileTime,
+           packetSize =unpacket_traits<PacketType>::size,
+           alignedSize = (size/packetSize)*packetSize };
+
+    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
+    copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
+  }
+};
+
+/**************************
+*** Inner vectorization ***
+**************************/
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling>
+{
+  typedef typename Kernel::PacketType PacketType;
+  enum {
+    SrcAlignment = Kernel::AssignmentTraits::SrcAlignment,
+    DstAlignment = Kernel::AssignmentTraits::DstAlignment
+  };
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    const Index innerSize = kernel.innerSize();
+    const Index outerSize = kernel.outerSize();
+    const Index packetSize = unpacket_traits<PacketType>::size;
+    for(Index outer = 0; outer < outerSize; ++outer)
+      for(Index inner = 0; inner < innerSize; inner+=packetSize)
+        kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, InnerUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    typedef typename Kernel::AssignmentTraits Traits;
+    const Index outerSize = kernel.outerSize();
+    for(Index outer = 0; outer < outerSize; ++outer)
+      copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, DstXprType::InnerSizeAtCompileTime,
+                                                   Traits::SrcAlignment, Traits::DstAlignment>::run(kernel, outer);
+  }
+};
+
+/***********************
+*** Linear traversal ***
+***********************/
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    const Index size = kernel.size();
+    for(Index i = 0; i < size; ++i)
+      kernel.assignCoeff(i);
+  }
+};
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+  }
+};
+
+/**************************
+*** Slice vectorization ***
+***************************/
+
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::Scalar Scalar;
+    typedef typename Kernel::PacketType PacketType;
+    enum {
+      packetSize = unpacket_traits<PacketType>::size,
+      requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
+      alignable = packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment)>=sizeof(Scalar),
+      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment)>=int(requestedAlignment),
+      dstAlignment = alignable ? int(requestedAlignment)
+                               : int(Kernel::AssignmentTraits::DstAlignment)
+    };
+    const Scalar *dst_ptr = kernel.dstDataPtr();
+    if((!bool(dstIsAligned)) && (UIntPtr(dst_ptr) % sizeof(Scalar))>0)
+    {
+      // the pointer is not aligend-on scalar, so alignment is not possible
+      return dense_assignment_loop<Kernel,DefaultTraversal,NoUnrolling>::run(kernel);
+    }
+    const Index packetAlignedMask = packetSize - 1;
+    const Index innerSize = kernel.innerSize();
+    const Index outerSize = kernel.outerSize();
+    const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
+    Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
+
+    for(Index outer = 0; outer < outerSize; ++outer)
+    {
+      const Index alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask);
+      // do the non-vectorizable part of the assignment
+      for(Index inner = 0; inner<alignedStart ; ++inner)
+        kernel.assignCoeffByOuterInner(outer, inner);
+
+      // do the vectorizable part of the assignment
+      for(Index inner = alignedStart; inner<alignedEnd; inner+=packetSize)
+        kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
+
+      // do the non-vectorizable part of the assignment
+      for(Index inner = alignedEnd; inner<innerSize ; ++inner)
+        kernel.assignCoeffByOuterInner(outer, inner);
+
+      alignedStart = numext::mini((alignedStart+alignedStep)%packetSize, innerSize);
+    }
+  }
+};
+
+#if EIGEN_UNALIGNED_VECTORIZE
+template<typename Kernel>
+struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, InnerUnrolling>
+{
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel &kernel)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    typedef typename Kernel::PacketType PacketType;
+
+    enum { size = DstXprType::InnerSizeAtCompileTime,
+           packetSize =unpacket_traits<PacketType>::size,
+           vectorizableSize = (size/packetSize)*packetSize };
+
+    for(Index outer = 0; outer < kernel.outerSize(); ++outer)
+    {
+      copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, vectorizableSize, 0, 0>::run(kernel, outer);
+      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, vectorizableSize, size>::run(kernel, outer);
+    }
+  }
+};
+#endif
+
+
+/***************************************************************************
+* Part 4 : Generic dense assignment kernel
+***************************************************************************/
+
+// This class generalize the assignment of a coefficient (or packet) from one dense evaluator
+// to another dense writable evaluator.
+// It is parametrized by the two evaluators, and the actual assignment functor.
+// This abstraction level permits to keep the evaluation loops as simple and as generic as possible.
+// One can customize the assignment using this generic dense_assignment_kernel with different
+// functors, or by completely overloading it, by-passing a functor.
+template<typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
+class generic_dense_assignment_kernel
+{
+protected:
+  typedef typename DstEvaluatorTypeT::XprType DstXprType;
+  typedef typename SrcEvaluatorTypeT::XprType SrcXprType;
+public:
+  
+  typedef DstEvaluatorTypeT DstEvaluatorType;
+  typedef SrcEvaluatorTypeT SrcEvaluatorType;
+  typedef typename DstEvaluatorType::Scalar Scalar;
+  typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
+  typedef typename AssignmentTraits::PacketType PacketType;
+  
+  
+  EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
+    : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr)
+  {
+    #ifdef EIGEN_DEBUG_ASSIGN
+    AssignmentTraits::debug();
+    #endif
+  }
+  
+  EIGEN_DEVICE_FUNC Index size() const        { return m_dstExpr.size(); }
+  EIGEN_DEVICE_FUNC Index innerSize() const   { return m_dstExpr.innerSize(); }
+  EIGEN_DEVICE_FUNC Index outerSize() const   { return m_dstExpr.outerSize(); }
+  EIGEN_DEVICE_FUNC Index rows() const        { return m_dstExpr.rows(); }
+  EIGEN_DEVICE_FUNC Index cols() const        { return m_dstExpr.cols(); }
+  EIGEN_DEVICE_FUNC Index outerStride() const { return m_dstExpr.outerStride(); }
+  
+  EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() { return m_dst; }
+  EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const { return m_src; }
+  
+  /// Assign src(row,col) to dst(row,col) through the assignment functor.
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col)
+  {
+    m_functor.assignCoeff(m_dst.coeffRef(row,col), m_src.coeff(row,col));
+  }
+  
+  /// \sa assignCoeff(Index,Index)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index)
+  {
+    m_functor.assignCoeff(m_dst.coeffRef(index), m_src.coeff(index));
+  }
+  
+  /// \sa assignCoeff(Index,Index)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner)
+  {
+    Index row = rowIndexByOuterInner(outer, inner); 
+    Index col = colIndexByOuterInner(outer, inner); 
+    assignCoeff(row, col);
+  }
+  
+  
+  template<int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col)
+  {
+    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(row,col), m_src.template packet<LoadMode,PacketType>(row,col));
+  }
+  
+  template<int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index)
+  {
+    m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(index), m_src.template packet<LoadMode,PacketType>(index));
+  }
+  
+  template<int StoreMode, int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner)
+  {
+    Index row = rowIndexByOuterInner(outer, inner); 
+    Index col = colIndexByOuterInner(outer, inner);
+    assignPacket<StoreMode,LoadMode,PacketType>(row, col);
+  }
+  
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner)
+  {
+    typedef typename DstEvaluatorType::ExpressionTraits Traits;
+    return int(Traits::RowsAtCompileTime) == 1 ? 0
+      : int(Traits::ColsAtCompileTime) == 1 ? inner
+      : int(DstEvaluatorType::Flags)&RowMajorBit ? outer
+      : inner;
+  }
+
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner)
+  {
+    typedef typename DstEvaluatorType::ExpressionTraits Traits;
+    return int(Traits::ColsAtCompileTime) == 1 ? 0
+      : int(Traits::RowsAtCompileTime) == 1 ? inner
+      : int(DstEvaluatorType::Flags)&RowMajorBit ? inner
+      : outer;
+  }
+
+  EIGEN_DEVICE_FUNC const Scalar* dstDataPtr() const
+  {
+    return m_dstExpr.data();
+  }
+  
+protected:
+  DstEvaluatorType& m_dst;
+  const SrcEvaluatorType& m_src;
+  const Functor &m_functor;
+  // TODO find a way to avoid the needs of the original expression
+  DstXprType& m_dstExpr;
+};
+
+/***************************************************************************
+* Part 5 : Entry point for dense rectangular assignment
+***************************************************************************/
+
+template<typename DstXprType,typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const Functor &/*func*/)
+{
+  EIGEN_ONLY_USED_FOR_DEBUG(dst);
+  EIGEN_ONLY_USED_FOR_DEBUG(src);
+  eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
+}
+
+template<typename DstXprType,typename SrcXprType, typename T1, typename T2>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void resize_if_allowed(DstXprType &dst, const SrcXprType& src, const internal::assign_op<T1,T2> &/*func*/)
+{
+  Index dstRows = src.rows();
+  Index dstCols = src.cols();
+  if(((dst.rows()!=dstRows) || (dst.cols()!=dstCols)))
+    dst.resize(dstRows, dstCols);
+  eigen_assert(dst.rows() == dstRows && dst.cols() == dstCols);
+}
+
+template<typename DstXprType, typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func)
+{
+  typedef evaluator<DstXprType> DstEvaluatorType;
+  typedef evaluator<SrcXprType> SrcEvaluatorType;
+
+  SrcEvaluatorType srcEvaluator(src);
+
+  // NOTE To properly handle A = (A*A.transpose())/s with A rectangular,
+  // we need to resize the destination after the source evaluator has been created.
+  resize_if_allowed(dst, src, func);
+
+  DstEvaluatorType dstEvaluator(dst);
+    
+  typedef generic_dense_assignment_kernel<DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
+  Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
+
+  dense_assignment_loop<Kernel>::run(kernel);
+}
+
+template<typename DstXprType, typename SrcXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src)
+{
+  call_dense_assignment_loop(dst, src, internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>());
+}
+
+/***************************************************************************
+* Part 6 : Generic assignment
+***************************************************************************/
+
+// Based on the respective shapes of the destination and source,
+// the class AssignmentKind determine the kind of assignment mechanism.
+// AssignmentKind must define a Kind typedef.
+template<typename DstShape, typename SrcShape> struct AssignmentKind;
+
+// Assignement kind defined in this file:
+struct Dense2Dense {};
+struct EigenBase2EigenBase {};
+
+template<typename,typename> struct AssignmentKind { typedef EigenBase2EigenBase Kind; };
+template<> struct AssignmentKind<DenseShape,DenseShape> { typedef Dense2Dense Kind; };
+    
+// This is the main assignment class
+template< typename DstXprType, typename SrcXprType, typename Functor,
+          typename Kind = typename AssignmentKind< typename evaluator_traits<DstXprType>::Shape , typename evaluator_traits<SrcXprType>::Shape >::Kind,
+          typename EnableIf = void>
+struct Assignment;
+
+
+// The only purpose of this call_assignment() function is to deal with noalias() / "assume-aliasing" and automatic transposition.
+// Indeed, I (Gael) think that this concept of "assume-aliasing" was a mistake, and it makes thing quite complicated.
+// So this intermediate function removes everything related to "assume-aliasing" such that Assignment
+// does not has to bother about these annoying details.
+
+template<typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment(Dst& dst, const Src& src)
+{
+  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+}
+template<typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment(const Dst& dst, const Src& src)
+{
+  call_assignment(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+}
+                     
+// Deal with "assume-aliasing"
+template<typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if< evaluator_assume_aliasing<Src>::value, void*>::type = 0)
+{
+  typename plain_matrix_type<Src>::type tmp(src);
+  call_assignment_no_alias(dst, tmp, func);
+}
+
+template<typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment(Dst& dst, const Src& src, const Func& func, typename enable_if<!evaluator_assume_aliasing<Src>::value, void*>::type = 0)
+{
+  call_assignment_no_alias(dst, src, func);
+}
+
+// by-pass "assume-aliasing"
+// When there is no aliasing, we require that 'dst' has been properly resized
+template<typename Dst, template <typename> class StorageBase, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment(NoAlias<Dst,StorageBase>& dst, const Src& src, const Func& func)
+{
+  call_assignment_no_alias(dst.expression(), src, func);
+}
+
+
+template<typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment_no_alias(Dst& dst, const Src& src, const Func& func)
+{
+  enum {
+    NeedToTranspose = (    (int(Dst::RowsAtCompileTime) == 1 && int(Src::ColsAtCompileTime) == 1)
+                        || (int(Dst::ColsAtCompileTime) == 1 && int(Src::RowsAtCompileTime) == 1)
+                      ) && int(Dst::SizeAtCompileTime) != 1
+  };
+
+  typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst>::type ActualDstTypeCleaned;
+  typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst&>::type ActualDstType;
+  ActualDstType actualDst(dst);
+  
+  // TODO check whether this is the right place to perform these checks:
+  EIGEN_STATIC_ASSERT_LVALUE(Dst)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar);
+  
+  Assignment<ActualDstTypeCleaned,Src,Func>::run(actualDst, src, func);
+}
+template<typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment_no_alias(Dst& dst, const Src& src)
+{
+  call_assignment_no_alias(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+}
+
+template<typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src, const Func& func)
+{
+  // TODO check whether this is the right place to perform these checks:
+  EIGEN_STATIC_ASSERT_LVALUE(Dst)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Dst,Src)
+  EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename Dst::Scalar,typename Src::Scalar);
+
+  Assignment<Dst,Src,Func>::run(dst, src, func);
+}
+template<typename Dst, typename Src>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_assignment_no_alias_no_transpose(Dst& dst, const Src& src)
+{
+  call_assignment_no_alias_no_transpose(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
+}
+
+// forward declaration
+template<typename Dst, typename Src> void check_for_aliasing(const Dst &dst, const Src &src);
+
+// Generic Dense to Dense assignment
+// Note that the last template argument "Weak" is needed to make it possible to perform
+// both partial specialization+SFINAE without ambiguous specialization
+template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak>
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+  {
+#ifndef EIGEN_NO_DEBUG
+    internal::check_for_aliasing(dst, src);
+#endif
+    
+    call_dense_assignment_loop(dst, src, func);
+  }
+};
+
+// Generic assignment through evalTo.
+// TODO: not sure we have to keep that one, but it helps porting current code to new evaluator mechanism.
+// Note that the last template argument "Weak" is needed to make it possible to perform
+// both partial specialization+SFINAE without ambiguous specialization
+template< typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
+struct Assignment<DstXprType, SrcXprType, Functor, EigenBase2EigenBase, Weak>
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar> &/*func*/)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
+    src.evalTo(dst);
+  }
+
+  // NOTE The following two functions are templated to avoid their instanciation if not needed
+  //      This is needed because some expressions supports evalTo only and/or have 'void' as scalar type.
+  template<typename SrcScalarType>
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<typename DstXprType::Scalar,SrcScalarType> &/*func*/)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
+    src.addTo(dst);
+  }
+
+  template<typename SrcScalarType>
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<typename DstXprType::Scalar,SrcScalarType> &/*func*/)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
+    src.subTo(dst);
+  }
+};
+
+} // namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_ASSIGN_EVALUATOR_H

examples/ThirdPartyLibs/Eigen/src/Core/Assign_MKL.h

@@ -0,0 +1,176 @@
+/*
+ Copyright (c) 2011, Intel Corporation. All rights reserved.
+ Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+ 
+ Redistribution and use in source and binary forms, with or without modification,
+ are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+ * Neither the name of Intel Corporation nor the names of its contributors may
+   be used to endorse or promote products derived from this software without
+   specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ ********************************************************************************
+ *   Content : Eigen bindings to Intel(R) MKL
+ *   MKL VML support for coefficient-wise unary Eigen expressions like a=b.sin()
+ ********************************************************************************
+*/
+
+#ifndef EIGEN_ASSIGN_VML_H
+#define EIGEN_ASSIGN_VML_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Dst, typename Src>
+class vml_assign_traits
+{
+  private:
+    enum {
+      DstHasDirectAccess = Dst::Flags & DirectAccessBit,
+      SrcHasDirectAccess = Src::Flags & DirectAccessBit,
+      StorageOrdersAgree = (int(Dst::IsRowMajor) == int(Src::IsRowMajor)),
+      InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime)
+                : int(Dst::Flags)&RowMajorBit ? int(Dst::ColsAtCompileTime)
+                : int(Dst::RowsAtCompileTime),
+      InnerMaxSize  = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime)
+                    : int(Dst::Flags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime)
+                    : int(Dst::MaxRowsAtCompileTime),
+      MaxSizeAtCompileTime = Dst::SizeAtCompileTime,
+
+      MightEnableVml = StorageOrdersAgree && DstHasDirectAccess && SrcHasDirectAccess && Src::InnerStrideAtCompileTime==1 && Dst::InnerStrideAtCompileTime==1,
+      MightLinearize = MightEnableVml && (int(Dst::Flags) & int(Src::Flags) & LinearAccessBit),
+      VmlSize = MightLinearize ? MaxSizeAtCompileTime : InnerMaxSize,
+      LargeEnough = VmlSize==Dynamic || VmlSize>=EIGEN_MKL_VML_THRESHOLD
+    };
+  public:
+    enum {
+      EnableVml = MightEnableVml && LargeEnough,
+      Traversal = MightLinearize ? LinearTraversal : DefaultTraversal
+    };
+};
+
+#define EIGEN_PP_EXPAND(ARG) ARG
+#if !defined (EIGEN_FAST_MATH) || (EIGEN_FAST_MATH != 1)
+#define EIGEN_VMLMODE_EXPAND_LA , VML_HA
+#else
+#define EIGEN_VMLMODE_EXPAND_LA , VML_LA
+#endif
+
+#define EIGEN_VMLMODE_EXPAND__ 
+
+#define EIGEN_VMLMODE_PREFIX_LA vm
+#define EIGEN_VMLMODE_PREFIX__  v
+#define EIGEN_VMLMODE_PREFIX(VMLMODE) EIGEN_CAT(EIGEN_VMLMODE_PREFIX_,VMLMODE)
+
+#define EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE)                                           \
+  template< typename DstXprType, typename SrcXprNested>                                                                         \
+  struct Assignment<DstXprType, CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested>, assign_op<EIGENTYPE,EIGENTYPE>,   \
+                   Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {              \
+    typedef CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested> SrcXprType;                                            \
+    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) {                   \
+      eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                       \
+      if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal) {                                              \
+        VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(),                                                        \
+              (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) );                                           \
+      } else {                                                                                                                  \
+        const Index outerSize = dst.outerSize();                                                                                \
+        for(Index outer = 0; outer < outerSize; ++outer) {                                                                      \
+          const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.nestedExpression().coeffRef(outer,0)) :                             \
+                                                      &(src.nestedExpression().coeffRef(0, outer));                             \
+          EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer));                           \
+          VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr,                                                                      \
+                (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE));                                             \
+        }                                                                                                                       \
+      }                                                                                                                         \
+    }                                                                                                                           \
+  };                                                                                                                            \
+
+
+#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE)                                                         \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),s##VMLOP), float, float, VMLMODE)           \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),d##VMLOP), double, double, VMLMODE)
+
+#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE)                                                         \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),c##VMLOP), scomplex, MKL_Complex8, VMLMODE) \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),z##VMLOP), dcomplex, MKL_Complex16, VMLMODE)
+  
+#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS(EIGENOP, VMLOP, VMLMODE)                                                              \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE)                                                               \
+  EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE)
+
+  
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sin,   Sin,   LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(asin,  Asin,  LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sinh,  Sinh,  LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cos,   Cos,   LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(acos,  Acos,  LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cosh,  Cosh,  LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tan,   Tan,   LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(atan,  Atan,  LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tanh,  Tanh,  LA)
+// EIGEN_MKL_VML_DECLARE_UNARY_CALLS(abs,   Abs,    _)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(exp,   Exp,   LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log,   Ln,    LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log10, Log10, LA)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sqrt,  Sqrt,  _)
+
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr,   _)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(arg, Arg,      _)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(round, Round,  _)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(floor, Floor,  _)
+EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil,  Ceil,   _)
+
+#define EIGEN_MKL_VML_DECLARE_POW_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE)                                           \
+  template< typename DstXprType, typename SrcXprNested, typename Plain>                                                       \
+  struct Assignment<DstXprType, CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested,                       \
+                    const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> >, assign_op<EIGENTYPE,EIGENTYPE>,    \
+                   Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {            \
+    typedef CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested,                                           \
+                    const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> > SrcXprType;                         \
+    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) {                 \
+      eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                     \
+      VMLTYPE exponent = reinterpret_cast<const VMLTYPE&>(src.rhs().functor().m_other);                                       \
+      if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal)                                              \
+      {                                                                                                                       \
+        VMLOP( dst.size(), (const VMLTYPE*)src.lhs().data(), exponent,                                                        \
+              (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) );                                         \
+      } else {                                                                                                                \
+        const Index outerSize = dst.outerSize();                                                                              \
+        for(Index outer = 0; outer < outerSize; ++outer) {                                                                    \
+          const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.lhs().coeffRef(outer,0)) :                                        \
+                                                      &(src.lhs().coeffRef(0, outer));                                        \
+          EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer));                         \
+          VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, exponent,                                                          \
+                 (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE));                                          \
+        }                                                                                                                     \
+      }                                                                                                                       \
+    }                                                                                                                         \
+  };
+  
+EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmsPowx, float,    float,         LA)
+EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdPowx, double,   double,        LA)
+EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcPowx, scomplex, MKL_Complex8,  LA)
+EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmzPowx, dcomplex, MKL_Complex16, LA)
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_ASSIGN_VML_H

examples/ThirdPartyLibs/Eigen/src/Core/BandMatrix.h

@@ -0,0 +1,353 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_BANDMATRIX_H
+#define EIGEN_BANDMATRIX_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Derived>
+class BandMatrixBase : public EigenBase<Derived>
+{
+  public:
+
+    enum {
+      Flags = internal::traits<Derived>::Flags,
+      CoeffReadCost = internal::traits<Derived>::CoeffReadCost,
+      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+      Supers = internal::traits<Derived>::Supers,
+      Subs   = internal::traits<Derived>::Subs,
+      Options = internal::traits<Derived>::Options
+    };
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> DenseMatrixType;
+    typedef typename DenseMatrixType::StorageIndex StorageIndex;
+    typedef typename internal::traits<Derived>::CoefficientsType CoefficientsType;
+    typedef EigenBase<Derived> Base;
+
+  protected:
+    enum {
+      DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic))
+                            ? 1 + Supers + Subs
+                            : Dynamic,
+      SizeAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime,ColsAtCompileTime)
+    };
+
+  public:
+    
+    using Base::derived;
+    using Base::rows;
+    using Base::cols;
+
+    /** \returns the number of super diagonals */
+    inline Index supers() const { return derived().supers(); }
+
+    /** \returns the number of sub diagonals */
+    inline Index subs() const { return derived().subs(); }
+    
+    /** \returns an expression of the underlying coefficient matrix */
+    inline const CoefficientsType& coeffs() const { return derived().coeffs(); }
+    
+    /** \returns an expression of the underlying coefficient matrix */
+    inline CoefficientsType& coeffs() { return derived().coeffs(); }
+
+    /** \returns a vector expression of the \a i -th column,
+      * only the meaningful part is returned.
+      * \warning the internal storage must be column major. */
+    inline Block<CoefficientsType,Dynamic,1> col(Index i)
+    {
+      EIGEN_STATIC_ASSERT((Options&RowMajor)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+      Index start = 0;
+      Index len = coeffs().rows();
+      if (i<=supers())
+      {
+        start = supers()-i;
+        len = (std::min)(rows(),std::max<Index>(0,coeffs().rows() - (supers()-i)));
+      }
+      else if (i>=rows()-subs())
+        len = std::max<Index>(0,coeffs().rows() - (i + 1 - rows() + subs()));
+      return Block<CoefficientsType,Dynamic,1>(coeffs(), start, i, len, 1);
+    }
+
+    /** \returns a vector expression of the main diagonal */
+    inline Block<CoefficientsType,1,SizeAtCompileTime> diagonal()
+    { return Block<CoefficientsType,1,SizeAtCompileTime>(coeffs(),supers(),0,1,(std::min)(rows(),cols())); }
+
+    /** \returns a vector expression of the main diagonal (const version) */
+    inline const Block<const CoefficientsType,1,SizeAtCompileTime> diagonal() const
+    { return Block<const CoefficientsType,1,SizeAtCompileTime>(coeffs(),supers(),0,1,(std::min)(rows(),cols())); }
+
+    template<int Index> struct DiagonalIntReturnType {
+      enum {
+        ReturnOpposite = (Options&SelfAdjoint) && (((Index)>0 && Supers==0) || ((Index)<0 && Subs==0)),
+        Conjugate = ReturnOpposite && NumTraits<Scalar>::IsComplex,
+        ActualIndex = ReturnOpposite ? -Index : Index,
+        DiagonalSize = (RowsAtCompileTime==Dynamic || ColsAtCompileTime==Dynamic)
+                     ? Dynamic
+                     : (ActualIndex<0
+                     ? EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime, RowsAtCompileTime + ActualIndex)
+                     : EIGEN_SIZE_MIN_PREFER_DYNAMIC(RowsAtCompileTime, ColsAtCompileTime - ActualIndex))
+      };
+      typedef Block<CoefficientsType,1, DiagonalSize> BuildType;
+      typedef typename internal::conditional<Conjugate,
+                 CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>,BuildType >,
+                 BuildType>::type Type;
+    };
+
+    /** \returns a vector expression of the \a N -th sub or super diagonal */
+    template<int N> inline typename DiagonalIntReturnType<N>::Type diagonal()
+    {
+      return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers()-N, (std::max)(0,N), 1, diagonalLength(N));
+    }
+
+    /** \returns a vector expression of the \a N -th sub or super diagonal */
+    template<int N> inline const typename DiagonalIntReturnType<N>::Type diagonal() const
+    {
+      return typename DiagonalIntReturnType<N>::BuildType(coeffs(), supers()-N, (std::max)(0,N), 1, diagonalLength(N));
+    }
+
+    /** \returns a vector expression of the \a i -th sub or super diagonal */
+    inline Block<CoefficientsType,1,Dynamic> diagonal(Index i)
+    {
+      eigen_assert((i<0 && -i<=subs()) || (i>=0 && i<=supers()));
+      return Block<CoefficientsType,1,Dynamic>(coeffs(), supers()-i, std::max<Index>(0,i), 1, diagonalLength(i));
+    }
+
+    /** \returns a vector expression of the \a i -th sub or super diagonal */
+    inline const Block<const CoefficientsType,1,Dynamic> diagonal(Index i) const
+    {
+      eigen_assert((i<0 && -i<=subs()) || (i>=0 && i<=supers()));
+      return Block<const CoefficientsType,1,Dynamic>(coeffs(), supers()-i, std::max<Index>(0,i), 1, diagonalLength(i));
+    }
+    
+    template<typename Dest> inline void evalTo(Dest& dst) const
+    {
+      dst.resize(rows(),cols());
+      dst.setZero();
+      dst.diagonal() = diagonal();
+      for (Index i=1; i<=supers();++i)
+        dst.diagonal(i) = diagonal(i);
+      for (Index i=1; i<=subs();++i)
+        dst.diagonal(-i) = diagonal(-i);
+    }
+
+    DenseMatrixType toDenseMatrix() const
+    {
+      DenseMatrixType res(rows(),cols());
+      evalTo(res);
+      return res;
+    }
+
+  protected:
+
+    inline Index diagonalLength(Index i) const
+    { return i<0 ? (std::min)(cols(),rows()+i) : (std::min)(rows(),cols()-i); }
+};
+
+/**
+  * \class BandMatrix
+  * \ingroup Core_Module
+  *
+  * \brief Represents a rectangular matrix with a banded storage
+  *
+  * \tparam _Scalar Numeric type, i.e. float, double, int
+  * \tparam _Rows Number of rows, or \b Dynamic
+  * \tparam _Cols Number of columns, or \b Dynamic
+  * \tparam _Supers Number of super diagonal
+  * \tparam _Subs Number of sub diagonal
+  * \tparam _Options A combination of either \b #RowMajor or \b #ColMajor, and of \b #SelfAdjoint
+  *                  The former controls \ref TopicStorageOrders "storage order", and defaults to
+  *                  column-major. The latter controls whether the matrix represents a selfadjoint
+  *                  matrix in which case either Supers of Subs have to be null.
+  *
+  * \sa class TridiagonalMatrix
+  */
+
+template<typename _Scalar, int _Rows, int _Cols, int _Supers, int _Subs, int _Options>
+struct traits<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
+{
+  typedef _Scalar Scalar;
+  typedef Dense StorageKind;
+  typedef Eigen::Index StorageIndex;
+  enum {
+    CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    RowsAtCompileTime = _Rows,
+    ColsAtCompileTime = _Cols,
+    MaxRowsAtCompileTime = _Rows,
+    MaxColsAtCompileTime = _Cols,
+    Flags = LvalueBit,
+    Supers = _Supers,
+    Subs = _Subs,
+    Options = _Options,
+    DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic)) ? 1 + Supers + Subs : Dynamic
+  };
+  typedef Matrix<Scalar,DataRowsAtCompileTime,ColsAtCompileTime,Options&RowMajor?RowMajor:ColMajor> CoefficientsType;
+};
+
+template<typename _Scalar, int Rows, int Cols, int Supers, int Subs, int Options>
+class BandMatrix : public BandMatrixBase<BandMatrix<_Scalar,Rows,Cols,Supers,Subs,Options> >
+{
+  public:
+
+    typedef typename internal::traits<BandMatrix>::Scalar Scalar;
+    typedef typename internal::traits<BandMatrix>::StorageIndex StorageIndex;
+    typedef typename internal::traits<BandMatrix>::CoefficientsType CoefficientsType;
+
+    explicit inline BandMatrix(Index rows=Rows, Index cols=Cols, Index supers=Supers, Index subs=Subs)
+      : m_coeffs(1+supers+subs,cols),
+        m_rows(rows), m_supers(supers), m_subs(subs)
+    {
+    }
+
+    /** \returns the number of columns */
+    inline Index rows() const { return m_rows.value(); }
+
+    /** \returns the number of rows */
+    inline Index cols() const { return m_coeffs.cols(); }
+
+    /** \returns the number of super diagonals */
+    inline Index supers() const { return m_supers.value(); }
+
+    /** \returns the number of sub diagonals */
+    inline Index subs() const { return m_subs.value(); }
+
+    inline const CoefficientsType& coeffs() const { return m_coeffs; }
+    inline CoefficientsType& coeffs() { return m_coeffs; }
+
+  protected:
+
+    CoefficientsType m_coeffs;
+    internal::variable_if_dynamic<Index, Rows>   m_rows;
+    internal::variable_if_dynamic<Index, Supers> m_supers;
+    internal::variable_if_dynamic<Index, Subs>   m_subs;
+};
+
+template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
+class BandMatrixWrapper;
+
+template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
+struct traits<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
+{
+  typedef typename _CoefficientsType::Scalar Scalar;
+  typedef typename _CoefficientsType::StorageKind StorageKind;
+  typedef typename _CoefficientsType::StorageIndex StorageIndex;
+  enum {
+    CoeffReadCost = internal::traits<_CoefficientsType>::CoeffReadCost,
+    RowsAtCompileTime = _Rows,
+    ColsAtCompileTime = _Cols,
+    MaxRowsAtCompileTime = _Rows,
+    MaxColsAtCompileTime = _Cols,
+    Flags = LvalueBit,
+    Supers = _Supers,
+    Subs = _Subs,
+    Options = _Options,
+    DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic)) ? 1 + Supers + Subs : Dynamic
+  };
+  typedef _CoefficientsType CoefficientsType;
+};
+
+template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
+class BandMatrixWrapper : public BandMatrixBase<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
+{
+  public:
+
+    typedef typename internal::traits<BandMatrixWrapper>::Scalar Scalar;
+    typedef typename internal::traits<BandMatrixWrapper>::CoefficientsType CoefficientsType;
+    typedef typename internal::traits<BandMatrixWrapper>::StorageIndex StorageIndex;
+
+    explicit inline BandMatrixWrapper(const CoefficientsType& coeffs, Index rows=_Rows, Index cols=_Cols, Index supers=_Supers, Index subs=_Subs)
+      : m_coeffs(coeffs),
+        m_rows(rows), m_supers(supers), m_subs(subs)
+    {
+      EIGEN_UNUSED_VARIABLE(cols);
+      //internal::assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows());
+    }
+
+    /** \returns the number of columns */
+    inline Index rows() const { return m_rows.value(); }
+
+    /** \returns the number of rows */
+    inline Index cols() const { return m_coeffs.cols(); }
+
+    /** \returns the number of super diagonals */
+    inline Index supers() const { return m_supers.value(); }
+
+    /** \returns the number of sub diagonals */
+    inline Index subs() const { return m_subs.value(); }
+
+    inline const CoefficientsType& coeffs() const { return m_coeffs; }
+
+  protected:
+
+    const CoefficientsType& m_coeffs;
+    internal::variable_if_dynamic<Index, _Rows>   m_rows;
+    internal::variable_if_dynamic<Index, _Supers> m_supers;
+    internal::variable_if_dynamic<Index, _Subs>   m_subs;
+};
+
+/**
+  * \class TridiagonalMatrix
+  * \ingroup Core_Module
+  *
+  * \brief Represents a tridiagonal matrix with a compact banded storage
+  *
+  * \tparam Scalar Numeric type, i.e. float, double, int
+  * \tparam Size Number of rows and cols, or \b Dynamic
+  * \tparam Options Can be 0 or \b SelfAdjoint
+  *
+  * \sa class BandMatrix
+  */
+template<typename Scalar, int Size, int Options>
+class TridiagonalMatrix : public BandMatrix<Scalar,Size,Size,Options&SelfAdjoint?0:1,1,Options|RowMajor>
+{
+    typedef BandMatrix<Scalar,Size,Size,Options&SelfAdjoint?0:1,1,Options|RowMajor> Base;
+    typedef typename Base::StorageIndex StorageIndex;
+  public:
+    explicit TridiagonalMatrix(Index size = Size) : Base(size,size,Options&SelfAdjoint?0:1,1) {}
+
+    inline typename Base::template DiagonalIntReturnType<1>::Type super()
+    { return Base::template diagonal<1>(); }
+    inline const typename Base::template DiagonalIntReturnType<1>::Type super() const
+    { return Base::template diagonal<1>(); }
+    inline typename Base::template DiagonalIntReturnType<-1>::Type sub()
+    { return Base::template diagonal<-1>(); }
+    inline const typename Base::template DiagonalIntReturnType<-1>::Type sub() const
+    { return Base::template diagonal<-1>(); }
+  protected:
+};
+
+
+struct BandShape {};
+
+template<typename _Scalar, int _Rows, int _Cols, int _Supers, int _Subs, int _Options>
+struct evaluator_traits<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
+  : public evaluator_traits_base<BandMatrix<_Scalar,_Rows,_Cols,_Supers,_Subs,_Options> >
+{
+  typedef BandShape Shape;
+};
+
+template<typename _CoefficientsType,int _Rows, int _Cols, int _Supers, int _Subs,int _Options>
+struct evaluator_traits<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
+  : public evaluator_traits_base<BandMatrixWrapper<_CoefficientsType,_Rows,_Cols,_Supers,_Subs,_Options> >
+{
+  typedef BandShape Shape;
+};
+
+template<> struct AssignmentKind<DenseShape,BandShape> { typedef EigenBase2EigenBase Kind; };
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_BANDMATRIX_H

examples/ThirdPartyLibs/Eigen/src/Core/Block.h

@@ -0,0 +1,452 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_BLOCK_H
+#define EIGEN_BLOCK_H
+
+namespace Eigen { 
+
+namespace internal {
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprType>
+{
+  typedef typename traits<XprType>::Scalar Scalar;
+  typedef typename traits<XprType>::StorageKind StorageKind;
+  typedef typename traits<XprType>::XprKind XprKind;
+  typedef typename ref_selector<XprType>::type XprTypeNested;
+  typedef typename remove_reference<XprTypeNested>::type _XprTypeNested;
+  enum{
+    MatrixRows = traits<XprType>::RowsAtCompileTime,
+    MatrixCols = traits<XprType>::ColsAtCompileTime,
+    RowsAtCompileTime = MatrixRows == 0 ? 0 : BlockRows,
+    ColsAtCompileTime = MatrixCols == 0 ? 0 : BlockCols,
+    MaxRowsAtCompileTime = BlockRows==0 ? 0
+                         : RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime)
+                         : int(traits<XprType>::MaxRowsAtCompileTime),
+    MaxColsAtCompileTime = BlockCols==0 ? 0
+                         : ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime)
+                         : int(traits<XprType>::MaxColsAtCompileTime),
+
+    XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
+    IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
+               : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
+               : XprTypeIsRowMajor,
+    HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
+    InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType
+                             ? int(inner_stride_at_compile_time<XprType>::ret)
+                             : int(outer_stride_at_compile_time<XprType>::ret),
+    OuterStrideAtCompileTime = HasSameStorageOrderAsXprType
+                             ? int(outer_stride_at_compile_time<XprType>::ret)
+                             : int(inner_stride_at_compile_time<XprType>::ret),
+
+    // FIXME, this traits is rather specialized for dense object and it needs to be cleaned further
+    FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
+    FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
+    Flags = (traits<XprType>::Flags & (DirectAccessBit | (InnerPanel?CompressedAccessBit:0))) | FlagsLvalueBit | FlagsRowMajorBit,
+    // FIXME DirectAccessBit should not be handled by expressions
+    // 
+    // Alignment is needed by MapBase's assertions
+    // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the respective evaluator
+    Alignment = 0
+  };
+};
+
+template<typename XprType, int BlockRows=Dynamic, int BlockCols=Dynamic, bool InnerPanel = false,
+         bool HasDirectAccess = internal::has_direct_access<XprType>::ret> class BlockImpl_dense;
+         
+} // end namespace internal
+
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel, typename StorageKind> class BlockImpl;
+
+/** \class Block
+  * \ingroup Core_Module
+  *
+  * \brief Expression of a fixed-size or dynamic-size block
+  *
+  * \tparam XprType the type of the expression in which we are taking a block
+  * \tparam BlockRows the number of rows of the block we are taking at compile time (optional)
+  * \tparam BlockCols the number of columns of the block we are taking at compile time (optional)
+  * \tparam InnerPanel is true, if the block maps to a set of rows of a row major matrix or
+  *         to set of columns of a column major matrix (optional). The parameter allows to determine
+  *         at compile time whether aligned access is possible on the block expression.
+  *
+  * This class represents an expression of either a fixed-size or dynamic-size block. It is the return
+  * type of DenseBase::block(Index,Index,Index,Index) and DenseBase::block<int,int>(Index,Index) and
+  * most of the time this is the only way it is used.
+  *
+  * However, if you want to directly maniputate block expressions,
+  * for instance if you want to write a function returning such an expression, you
+  * will need to use this class.
+  *
+  * Here is an example illustrating the dynamic case:
+  * \include class_Block.cpp
+  * Output: \verbinclude class_Block.out
+  *
+  * \note Even though this expression has dynamic size, in the case where \a XprType
+  * has fixed size, this expression inherits a fixed maximal size which means that evaluating
+  * it does not cause a dynamic memory allocation.
+  *
+  * Here is an example illustrating the fixed-size case:
+  * \include class_FixedBlock.cpp
+  * Output: \verbinclude class_FixedBlock.out
+  *
+  * \sa DenseBase::block(Index,Index,Index,Index), DenseBase::block(Index,Index), class VectorBlock
+  */
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel> class Block
+  : public BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind>
+{
+    typedef BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, typename internal::traits<XprType>::StorageKind> Impl;
+  public:
+    //typedef typename Impl::Base Base;
+    typedef Impl Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
+    
+    typedef typename internal::remove_all<XprType>::type NestedExpression;
+  
+    /** Column or Row constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline Block(XprType& xpr, Index i) : Impl(xpr,i)
+    {
+      eigen_assert( (i>=0) && (
+          ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i<xpr.rows())
+        ||((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && i<xpr.cols())));
+    }
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline Block(XprType& xpr, Index startRow, Index startCol)
+      : Impl(xpr, startRow, startCol)
+    {
+      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+      eigen_assert(startRow >= 0 && BlockRows >= 0 && startRow + BlockRows <= xpr.rows()
+             && startCol >= 0 && BlockCols >= 0 && startCol + BlockCols <= xpr.cols());
+    }
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline Block(XprType& xpr,
+          Index startRow, Index startCol,
+          Index blockRows, Index blockCols)
+      : Impl(xpr, startRow, startCol, blockRows, blockCols)
+    {
+      eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
+          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
+      eigen_assert(startRow >= 0 && blockRows >= 0 && startRow  <= xpr.rows() - blockRows
+          && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols);
+    }
+};
+         
+// The generic default implementation for dense block simplu forward to the internal::BlockImpl_dense
+// that must be specialized for direct and non-direct access...
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl<XprType, BlockRows, BlockCols, InnerPanel, Dense>
+  : public internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel>
+{
+    typedef internal::BlockImpl_dense<XprType, BlockRows, BlockCols, InnerPanel> Impl;
+    typedef typename XprType::StorageIndex StorageIndex;
+  public:
+    typedef Impl Base;
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
+    EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {}
+    EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {}
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+      : Impl(xpr, startRow, startCol, blockRows, blockCols) {}
+};
+
+namespace internal {
+
+/** \internal Internal implementation of dense Blocks in the general case. */
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel, bool HasDirectAccess> class BlockImpl_dense
+  : public internal::dense_xpr_base<Block<XprType, BlockRows, BlockCols, InnerPanel> >::type
+{
+    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
+  public:
+
+    typedef typename internal::dense_xpr_base<BlockType>::type Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
+
+    // class InnerIterator; // FIXME apparently never used
+
+    /** Column or Row constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr, Index i)
+      : m_xpr(xpr),
+        // It is a row if and only if BlockRows==1 and BlockCols==XprType::ColsAtCompileTime,
+        // and it is a column if and only if BlockRows==XprType::RowsAtCompileTime and BlockCols==1,
+        // all other cases are invalid.
+        // The case a 1x1 matrix seems ambiguous, but the result is the same anyway.
+        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0),
+        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
+        m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
+        m_blockCols(BlockCols==1 ? 1 : xpr.cols())
+    {}
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
+      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol),
+                    m_blockRows(BlockRows), m_blockCols(BlockCols)
+    {}
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr,
+          Index startRow, Index startCol,
+          Index blockRows, Index blockCols)
+      : m_xpr(xpr), m_startRow(startRow), m_startCol(startCol),
+                    m_blockRows(blockRows), m_blockCols(blockCols)
+    {}
+
+    EIGEN_DEVICE_FUNC inline Index rows() const { return m_blockRows.value(); }
+    EIGEN_DEVICE_FUNC inline Index cols() const { return m_blockCols.value(); }
+
+    EIGEN_DEVICE_FUNC
+    inline Scalar& coeffRef(Index rowId, Index colId)
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(XprType)
+      return m_xpr.coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    {
+      return m_xpr.derived().coeffRef(rowId + m_startRow.value(), colId + m_startCol.value());
+    }
+
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const
+    {
+      return m_xpr.coeff(rowId + m_startRow.value(), colId + m_startCol.value());
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline Scalar& coeffRef(Index index)
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(XprType)
+      return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index index) const
+    {
+      return m_xpr.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                            m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const CoeffReturnType coeff(Index index) const
+    {
+      return m_xpr.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                         m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(Index rowId, Index colId) const
+    {
+      return m_xpr.template packet<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value());
+    }
+
+    template<int LoadMode>
+    inline void writePacket(Index rowId, Index colId, const PacketScalar& val)
+    {
+      m_xpr.template writePacket<Unaligned>(rowId + m_startRow.value(), colId + m_startCol.value(), val);
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(Index index) const
+    {
+      return m_xpr.template packet<Unaligned>
+              (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+               m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    template<int LoadMode>
+    inline void writePacket(Index index, const PacketScalar& val)
+    {
+      m_xpr.template writePacket<Unaligned>
+         (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), val);
+    }
+
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** \sa MapBase::data() */
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const;
+    EIGEN_DEVICE_FUNC inline Index innerStride() const;
+    EIGEN_DEVICE_FUNC inline Index outerStride() const;
+    #endif
+
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
+    { 
+      return m_xpr; 
+    }
+
+    EIGEN_DEVICE_FUNC
+    XprType& nestedExpression() { return m_xpr; }
+      
+    EIGEN_DEVICE_FUNC
+    StorageIndex startRow() const
+    { 
+      return m_startRow.value(); 
+    }
+      
+    EIGEN_DEVICE_FUNC
+    StorageIndex startCol() const
+    { 
+      return m_startCol.value(); 
+    }
+
+  protected:
+
+    XprTypeNested m_xpr;
+    const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
+    const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
+    const internal::variable_if_dynamic<StorageIndex, RowsAtCompileTime> m_blockRows;
+    const internal::variable_if_dynamic<StorageIndex, ColsAtCompileTime> m_blockCols;
+};
+
+/** \internal Internal implementation of dense Blocks in the direct access case.*/
+template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+class BlockImpl_dense<XprType,BlockRows,BlockCols, InnerPanel,true>
+  : public MapBase<Block<XprType, BlockRows, BlockCols, InnerPanel> >
+{
+    typedef Block<XprType, BlockRows, BlockCols, InnerPanel> BlockType;
+    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
+    enum {
+      XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0
+    };
+  public:
+
+    typedef MapBase<BlockType> Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(BlockType)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl_dense)
+
+    /** Column or Row constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr, Index i)
+      : Base(xpr.data() + i * (    ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor)) 
+                                || ((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && ( XprTypeIsRowMajor)) ? xpr.innerStride() : xpr.outerStride()),
+             BlockRows==1 ? 1 : xpr.rows(),
+             BlockCols==1 ? 1 : xpr.cols()),
+        m_xpr(xpr),
+        m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0),
+        m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0)
+    {
+      init();
+    }
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
+      : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol)),
+        m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
+    {
+      init();
+    }
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr,
+          Index startRow, Index startCol,
+          Index blockRows, Index blockCols)
+      : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol), blockRows, blockCols),
+        m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
+    {
+      init();
+    }
+
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
+    { 
+      return m_xpr; 
+    }
+
+    EIGEN_DEVICE_FUNC
+    XprType& nestedExpression() { return m_xpr; }
+      
+    /** \sa MapBase::innerStride() */
+    EIGEN_DEVICE_FUNC
+    inline Index innerStride() const
+    {
+      return internal::traits<BlockType>::HasSameStorageOrderAsXprType
+             ? m_xpr.innerStride()
+             : m_xpr.outerStride();
+    }
+
+    /** \sa MapBase::outerStride() */
+    EIGEN_DEVICE_FUNC
+    inline Index outerStride() const
+    {
+      return m_outerStride;
+    }
+
+    EIGEN_DEVICE_FUNC
+    StorageIndex startRow() const
+    {
+      return m_startRow.value();
+    }
+
+    EIGEN_DEVICE_FUNC
+    StorageIndex startCol() const
+    {
+      return m_startCol.value();
+    }
+
+  #ifndef __SUNPRO_CC
+  // FIXME sunstudio is not friendly with the above friend...
+  // META-FIXME there is no 'friend' keyword around here. Is this obsolete?
+  protected:
+  #endif
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal used by allowAligned() */
+    EIGEN_DEVICE_FUNC
+    inline BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols)
+      : Base(data, blockRows, blockCols), m_xpr(xpr)
+    {
+      init();
+    }
+    #endif
+
+  protected:
+    EIGEN_DEVICE_FUNC
+    void init()
+    {
+      m_outerStride = internal::traits<BlockType>::HasSameStorageOrderAsXprType
+                    ? m_xpr.outerStride()
+                    : m_xpr.innerStride();
+    }
+
+    XprTypeNested m_xpr;
+    const internal::variable_if_dynamic<StorageIndex, (XprType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
+    const internal::variable_if_dynamic<StorageIndex, (XprType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
+    Index m_outerStride;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_BLOCK_H

examples/ThirdPartyLibs/Eigen/src/Core/BooleanRedux.h

@@ -0,0 +1,162 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ALLANDANY_H
+#define EIGEN_ALLANDANY_H
+
+namespace Eigen { 
+
+namespace internal {
+
+template<typename Derived, int UnrollCount, int Rows>
+struct all_unroller
+{
+  enum {
+    col = (UnrollCount-1) / Rows,
+    row = (UnrollCount-1) % Rows
+  };
+
+  static inline bool run(const Derived &mat)
+  {
+    return all_unroller<Derived, UnrollCount-1, Rows>::run(mat) && mat.coeff(row, col);
+  }
+};
+
+template<typename Derived, int Rows>
+struct all_unroller<Derived, 0, Rows>
+{
+  static inline bool run(const Derived &/*mat*/) { return true; }
+};
+
+template<typename Derived, int Rows>
+struct all_unroller<Derived, Dynamic, Rows>
+{
+  static inline bool run(const Derived &) { return false; }
+};
+
+template<typename Derived, int UnrollCount, int Rows>
+struct any_unroller
+{
+  enum {
+    col = (UnrollCount-1) / Rows,
+    row = (UnrollCount-1) % Rows
+  };
+  
+  static inline bool run(const Derived &mat)
+  {
+    return any_unroller<Derived, UnrollCount-1, Rows>::run(mat) || mat.coeff(row, col);
+  }
+};
+
+template<typename Derived, int Rows>
+struct any_unroller<Derived, 0, Rows>
+{
+  static inline bool run(const Derived & /*mat*/) { return false; }
+};
+
+template<typename Derived, int Rows>
+struct any_unroller<Derived, Dynamic, Rows>
+{
+  static inline bool run(const Derived &) { return false; }
+};
+
+} // end namespace internal
+
+/** \returns true if all coefficients are true
+  *
+  * Example: \include MatrixBase_all.cpp
+  * Output: \verbinclude MatrixBase_all.out
+  *
+  * \sa any(), Cwise::operator<()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::all() const
+{
+  typedef internal::evaluator<Derived> Evaluator;
+  enum {
+    unroll = SizeAtCompileTime != Dynamic
+          && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits<Scalar>::AddCost) <= EIGEN_UNROLLING_LIMIT
+  };
+  Evaluator evaluator(derived());
+  if(unroll)
+    return internal::all_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
+  else
+  {
+    for(Index j = 0; j < cols(); ++j)
+      for(Index i = 0; i < rows(); ++i)
+        if (!evaluator.coeff(i, j)) return false;
+    return true;
+  }
+}
+
+/** \returns true if at least one coefficient is true
+  *
+  * \sa all()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::any() const
+{
+  typedef internal::evaluator<Derived> Evaluator;
+  enum {
+    unroll = SizeAtCompileTime != Dynamic
+          && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits<Scalar>::AddCost) <= EIGEN_UNROLLING_LIMIT
+  };
+  Evaluator evaluator(derived());
+  if(unroll)
+    return internal::any_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
+  else
+  {
+    for(Index j = 0; j < cols(); ++j)
+      for(Index i = 0; i < rows(); ++i)
+        if (evaluator.coeff(i, j)) return true;
+    return false;
+  }
+}
+
+/** \returns the number of coefficients which evaluate to true
+  *
+  * \sa all(), any()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC inline Eigen::Index DenseBase<Derived>::count() const
+{
+  return derived().template cast<bool>().template cast<Index>().sum();
+}
+
+/** \returns true is \c *this contains at least one Not A Number (NaN).
+  *
+  * \sa allFinite()
+  */
+template<typename Derived>
+inline bool DenseBase<Derived>::hasNaN() const
+{
+#if EIGEN_COMP_MSVC || (defined __FAST_MATH__)
+  return derived().array().isNaN().any();
+#else
+  return !((derived().array()==derived().array()).all());
+#endif
+}
+
+/** \returns true if \c *this contains only finite numbers, i.e., no NaN and no +/-INF values.
+  *
+  * \sa hasNaN()
+  */
+template<typename Derived>
+inline bool DenseBase<Derived>::allFinite() const
+{
+#if EIGEN_COMP_MSVC || (defined __FAST_MATH__)
+  return derived().array().isFinite().all();
+#else
+  return !((derived()-derived()).hasNaN());
+#endif
+}
+    
+} // end namespace Eigen
+
+#endif // EIGEN_ALLANDANY_H

examples/ThirdPartyLibs/Eigen/src/Core/CommaInitializer.h

@@ -0,0 +1,160 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_COMMAINITIALIZER_H
+#define EIGEN_COMMAINITIALIZER_H
+
+namespace Eigen { 
+
+/** \class CommaInitializer
+  * \ingroup Core_Module
+  *
+  * \brief Helper class used by the comma initializer operator
+  *
+  * This class is internally used to implement the comma initializer feature. It is
+  * the return type of MatrixBase::operator<<, and most of the time this is the only
+  * way it is used.
+  *
+  * \sa \blank \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
+  */
+template<typename XprType>
+struct CommaInitializer
+{
+  typedef typename XprType::Scalar Scalar;
+
+  EIGEN_DEVICE_FUNC
+  inline CommaInitializer(XprType& xpr, const Scalar& s)
+    : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
+  {
+    m_xpr.coeffRef(0,0) = s;
+  }
+
+  template<typename OtherDerived>
+  EIGEN_DEVICE_FUNC
+  inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
+    : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
+  {
+    m_xpr.block(0, 0, other.rows(), other.cols()) = other;
+  }
+
+  /* Copy/Move constructor which transfers ownership. This is crucial in 
+   * absence of return value optimization to avoid assertions during destruction. */
+  // FIXME in C++11 mode this could be replaced by a proper RValue constructor
+  EIGEN_DEVICE_FUNC
+  inline CommaInitializer(const CommaInitializer& o)
+  : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
+    // Mark original object as finished. In absence of R-value references we need to const_cast:
+    const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
+    const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
+    const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
+  }
+
+  /* inserts a scalar value in the target matrix */
+  EIGEN_DEVICE_FUNC
+  CommaInitializer& operator,(const Scalar& s)
+  {
+    if (m_col==m_xpr.cols())
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = 1;
+      eigen_assert(m_row<m_xpr.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    eigen_assert(m_col<m_xpr.cols()
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows==1);
+    m_xpr.coeffRef(m_row, m_col++) = s;
+    return *this;
+  }
+
+  /* inserts a matrix expression in the target matrix */
+  template<typename OtherDerived>
+  EIGEN_DEVICE_FUNC
+  CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
+  {
+    if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows))
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = other.rows();
+      eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    eigen_assert((m_col + other.cols() <= m_xpr.cols())
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows==other.rows());
+    m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>
+                    (m_row, m_col, other.rows(), other.cols()) = other;
+    m_col += other.cols();
+    return *this;
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline ~CommaInitializer()
+#if defined VERIFY_RAISES_ASSERT && (!defined EIGEN_NO_ASSERTION_CHECKING) && defined EIGEN_EXCEPTIONS
+  EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception)
+#endif
+  {
+      finished();
+  }
+
+  /** \returns the built matrix once all its coefficients have been set.
+    * Calling finished is 100% optional. Its purpose is to write expressions
+    * like this:
+    * \code
+    * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
+    * \endcode
+    */
+  EIGEN_DEVICE_FUNC
+  inline XprType& finished() {
+      eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0)
+           && m_col == m_xpr.cols()
+           && "Too few coefficients passed to comma initializer (operator<<)");
+      return m_xpr;
+  }
+
+  XprType& m_xpr;           // target expression
+  Index m_row;              // current row id
+  Index m_col;              // current col id
+  Index m_currentBlockRows; // current block height
+};
+
+/** \anchor MatrixBaseCommaInitRef
+  * Convenient operator to set the coefficients of a matrix.
+  *
+  * The coefficients must be provided in a row major order and exactly match
+  * the size of the matrix. Otherwise an assertion is raised.
+  *
+  * Example: \include MatrixBase_set.cpp
+  * Output: \verbinclude MatrixBase_set.out
+  * 
+  * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order.
+  *
+  * \sa CommaInitializer::finished(), class CommaInitializer
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
+}
+
+/** \sa operator<<(const Scalar&) */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived>
+DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_COMMAINITIALIZER_H

examples/ThirdPartyLibs/Eigen/src/Core/ConditionEstimator.h

@@ -0,0 +1,175 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2016 Rasmus Munk Larsen (rmlarsen@google.com)
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CONDITIONESTIMATOR_H
+#define EIGEN_CONDITIONESTIMATOR_H
+
+namespace Eigen {
+
+namespace internal {
+
+template <typename Vector, typename RealVector, bool IsComplex>
+struct rcond_compute_sign {
+  static inline Vector run(const Vector& v) {
+    const RealVector v_abs = v.cwiseAbs();
+    return (v_abs.array() == static_cast<typename Vector::RealScalar>(0))
+            .select(Vector::Ones(v.size()), v.cwiseQuotient(v_abs));
+  }
+};
+
+// Partial specialization to avoid elementwise division for real vectors.
+template <typename Vector>
+struct rcond_compute_sign<Vector, Vector, false> {
+  static inline Vector run(const Vector& v) {
+    return (v.array() < static_cast<typename Vector::RealScalar>(0))
+           .select(-Vector::Ones(v.size()), Vector::Ones(v.size()));
+  }
+};
+
+/**
+  * \returns an estimate of ||inv(matrix)||_1 given a decomposition of
+  * \a matrix that implements .solve() and .adjoint().solve() methods.
+  *
+  * This function implements Algorithms 4.1 and 5.1 from
+  *   http://www.maths.manchester.ac.uk/~higham/narep/narep135.pdf
+  * which also forms the basis for the condition number estimators in
+  * LAPACK. Since at most 10 calls to the solve method of dec are
+  * performed, the total cost is O(dims^2), as opposed to O(dims^3)
+  * needed to compute the inverse matrix explicitly.
+  *
+  * The most common usage is in estimating the condition number
+  * ||matrix||_1 * ||inv(matrix)||_1. The first term ||matrix||_1 can be
+  * computed directly in O(n^2) operations.
+  *
+  * Supports the following decompositions: FullPivLU, PartialPivLU, LDLT, and
+  * LLT.
+  *
+  * \sa FullPivLU, PartialPivLU, LDLT, LLT.
+  */
+template <typename Decomposition>
+typename Decomposition::RealScalar rcond_invmatrix_L1_norm_estimate(const Decomposition& dec)
+{
+  typedef typename Decomposition::MatrixType MatrixType;
+  typedef typename Decomposition::Scalar Scalar;
+  typedef typename Decomposition::RealScalar RealScalar;
+  typedef typename internal::plain_col_type<MatrixType>::type Vector;
+  typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVector;
+  const bool is_complex = (NumTraits<Scalar>::IsComplex != 0);
+
+  eigen_assert(dec.rows() == dec.cols());
+  const Index n = dec.rows();
+  if (n == 0)
+    return 0;
+
+  // Disable Index to float conversion warning
+#ifdef __INTEL_COMPILER
+  #pragma warning push
+  #pragma warning ( disable : 2259 )
+#endif
+  Vector v = dec.solve(Vector::Ones(n) / Scalar(n));
+#ifdef __INTEL_COMPILER
+  #pragma warning pop
+#endif
+
+  // lower_bound is a lower bound on
+  //   ||inv(matrix)||_1  = sup_v ||inv(matrix) v||_1 / ||v||_1
+  // and is the objective maximized by the ("super-") gradient ascent
+  // algorithm below.
+  RealScalar lower_bound = v.template lpNorm<1>();
+  if (n == 1)
+    return lower_bound;
+
+  // Gradient ascent algorithm follows: We know that the optimum is achieved at
+  // one of the simplices v = e_i, so in each iteration we follow a
+  // super-gradient to move towards the optimal one.
+  RealScalar old_lower_bound = lower_bound;
+  Vector sign_vector(n);
+  Vector old_sign_vector;
+  Index v_max_abs_index = -1;
+  Index old_v_max_abs_index = v_max_abs_index;
+  for (int k = 0; k < 4; ++k)
+  {
+    sign_vector = internal::rcond_compute_sign<Vector, RealVector, is_complex>::run(v);
+    if (k > 0 && !is_complex && sign_vector == old_sign_vector) {
+      // Break if the solution stagnated.
+      break;
+    }
+    // v_max_abs_index = argmax |real( inv(matrix)^T * sign_vector )|
+    v = dec.adjoint().solve(sign_vector);
+    v.real().cwiseAbs().maxCoeff(&v_max_abs_index);
+    if (v_max_abs_index == old_v_max_abs_index) {
+      // Break if the solution stagnated.
+      break;
+    }
+    // Move to the new simplex e_j, where j = v_max_abs_index.
+    v = dec.solve(Vector::Unit(n, v_max_abs_index));  // v = inv(matrix) * e_j.
+    lower_bound = v.template lpNorm<1>();
+    if (lower_bound <= old_lower_bound) {
+      // Break if the gradient step did not increase the lower_bound.
+      break;
+    }
+    if (!is_complex) {
+      old_sign_vector = sign_vector;
+    }
+    old_v_max_abs_index = v_max_abs_index;
+    old_lower_bound = lower_bound;
+  }
+  // The following calculates an independent estimate of ||matrix||_1 by
+  // multiplying matrix by a vector with entries of slowly increasing
+  // magnitude and alternating sign:
+  //   v_i = (-1)^{i} (1 + (i / (dim-1))), i = 0,...,dim-1.
+  // This improvement to Hager's algorithm above is due to Higham. It was
+  // added to make the algorithm more robust in certain corner cases where
+  // large elements in the matrix might otherwise escape detection due to
+  // exact cancellation (especially when op and op_adjoint correspond to a
+  // sequence of backsubstitutions and permutations), which could cause
+  // Hager's algorithm to vastly underestimate ||matrix||_1.
+  Scalar alternating_sign(RealScalar(1));
+  for (Index i = 0; i < n; ++i) {
+    // The static_cast is needed when Scalar is a complex and RealScalar implements expression templates
+    v[i] = alternating_sign * static_cast<RealScalar>(RealScalar(1) + (RealScalar(i) / (RealScalar(n - 1))));
+    alternating_sign = -alternating_sign;
+  }
+  v = dec.solve(v);
+  const RealScalar alternate_lower_bound = (2 * v.template lpNorm<1>()) / (3 * RealScalar(n));
+  return numext::maxi(lower_bound, alternate_lower_bound);
+}
+
+/** \brief Reciprocal condition number estimator.
+  *
+  * Computing a decomposition of a dense matrix takes O(n^3) operations, while
+  * this method estimates the condition number quickly and reliably in O(n^2)
+  * operations.
+  *
+  * \returns an estimate of the reciprocal condition number
+  * (1 / (||matrix||_1 * ||inv(matrix)||_1)) of matrix, given ||matrix||_1 and
+  * its decomposition. Supports the following decompositions: FullPivLU,
+  * PartialPivLU, LDLT, and LLT.
+  *
+  * \sa FullPivLU, PartialPivLU, LDLT, LLT.
+  */
+template <typename Decomposition>
+typename Decomposition::RealScalar
+rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Decomposition& dec)
+{
+  typedef typename Decomposition::RealScalar RealScalar;
+  eigen_assert(dec.rows() == dec.cols());
+  if (dec.rows() == 0)              return RealScalar(1);
+  if (matrix_norm == RealScalar(0)) return RealScalar(0);
+  if (dec.rows() == 1)              return RealScalar(1);
+  const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec);
+  return (inverse_matrix_norm == RealScalar(0) ? RealScalar(0)
+                                               : (RealScalar(1) / inverse_matrix_norm) / matrix_norm);
+}
+
+}  // namespace internal
+
+}  // namespace Eigen
+
+#endif

examples/ThirdPartyLibs/Eigen/src/Core/CoreIterators.h

@@ -0,0 +1,132 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_COREITERATORS_H
+#define EIGEN_COREITERATORS_H
+
+namespace Eigen { 
+
+/* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core
+ */
+
+namespace internal {
+
+template<typename XprType, typename EvaluatorKind>
+class inner_iterator_selector;
+
+}
+
+/** \class InnerIterator
+  * \brief An InnerIterator allows to loop over the element of any matrix expression.
+  * 
+  * \warning To be used with care because an evaluator is constructed every time an InnerIterator iterator is constructed.
+  * 
+  * TODO: add a usage example
+  */
+template<typename XprType>
+class InnerIterator
+{
+protected:
+  typedef internal::inner_iterator_selector<XprType, typename internal::evaluator_traits<XprType>::Kind> IteratorType;
+  typedef internal::evaluator<XprType> EvaluatorType;
+  typedef typename internal::traits<XprType>::Scalar Scalar;
+public:
+  /** Construct an iterator over the \a outerId -th row or column of \a xpr */
+  InnerIterator(const XprType &xpr, const Index &outerId)
+    : m_eval(xpr), m_iter(m_eval, outerId, xpr.innerSize())
+  {}
+  
+  /// \returns the value of the current coefficient.
+  EIGEN_STRONG_INLINE Scalar value() const          { return m_iter.value(); }
+  /** Increment the iterator \c *this to the next non-zero coefficient.
+    * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView
+    */
+  EIGEN_STRONG_INLINE InnerIterator& operator++()   { m_iter.operator++(); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator& operator+=(Index i) { m_iter.operator+=(i); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator operator+(Index i) 
+  { InnerIterator result(*this); result+=i; return result; }
+    
+
+  /// \returns the column or row index of the current coefficient.
+  EIGEN_STRONG_INLINE Index index() const           { return m_iter.index(); }
+  /// \returns the row index of the current coefficient.
+  EIGEN_STRONG_INLINE Index row() const             { return m_iter.row(); }
+  /// \returns the column index of the current coefficient.
+  EIGEN_STRONG_INLINE Index col() const             { return m_iter.col(); }
+  /// \returns \c true if the iterator \c *this still references a valid coefficient.
+  EIGEN_STRONG_INLINE operator bool() const         { return m_iter; }
+  
+protected:
+  EvaluatorType m_eval;
+  IteratorType m_iter;
+private:
+  // If you get here, then you're not using the right InnerIterator type, e.g.:
+  //   SparseMatrix<double,RowMajor> A;
+  //   SparseMatrix<double>::InnerIterator it(A,0);
+  template<typename T> InnerIterator(const EigenBase<T>&,Index outer);
+};
+
+namespace internal {
+
+// Generic inner iterator implementation for dense objects
+template<typename XprType>
+class inner_iterator_selector<XprType, IndexBased>
+{
+protected:
+  typedef evaluator<XprType> EvaluatorType;
+  typedef typename traits<XprType>::Scalar Scalar;
+  enum { IsRowMajor = (XprType::Flags&RowMajorBit)==RowMajorBit };
+  
+public:
+  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &innerSize)
+    : m_eval(eval), m_inner(0), m_outer(outerId), m_end(innerSize)
+  {}
+
+  EIGEN_STRONG_INLINE Scalar value() const
+  {
+    return (IsRowMajor) ? m_eval.coeff(m_outer, m_inner)
+                        : m_eval.coeff(m_inner, m_outer);
+  }
+
+  EIGEN_STRONG_INLINE inner_iterator_selector& operator++() { m_inner++; return *this; }
+
+  EIGEN_STRONG_INLINE Index index() const { return m_inner; }
+  inline Index row() const { return IsRowMajor ? m_outer : index(); }
+  inline Index col() const { return IsRowMajor ? index() : m_outer; }
+
+  EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+
+protected:
+  const EvaluatorType& m_eval;
+  Index m_inner;
+  const Index m_outer;
+  const Index m_end;
+};
+
+// For iterator-based evaluator, inner-iterator is already implemented as
+// evaluator<>::InnerIterator
+template<typename XprType>
+class inner_iterator_selector<XprType, IteratorBased>
+ : public evaluator<XprType>::InnerIterator
+{
+protected:
+  typedef typename evaluator<XprType>::InnerIterator Base;
+  typedef evaluator<XprType> EvaluatorType;
+  
+public:
+  EIGEN_STRONG_INLINE inner_iterator_selector(const EvaluatorType &eval, const Index &outerId, const Index &/*innerSize*/)
+    : Base(eval, outerId)
+  {}  
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_COREITERATORS_H

examples/ThirdPartyLibs/Eigen/src/Core/CwiseBinaryOp.h

@@ -0,0 +1,183 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_BINARY_OP_H
+#define EIGEN_CWISE_BINARY_OP_H
+
+namespace Eigen {
+
+namespace internal {
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  // we must not inherit from traits<Lhs> since it has
+  // the potential to cause problems with MSVC
+  typedef typename remove_all<Lhs>::type Ancestor;
+  typedef typename traits<Ancestor>::XprKind XprKind;
+  enum {
+    RowsAtCompileTime = traits<Ancestor>::RowsAtCompileTime,
+    ColsAtCompileTime = traits<Ancestor>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = traits<Ancestor>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = traits<Ancestor>::MaxColsAtCompileTime
+  };
+
+  // even though we require Lhs and Rhs to have the same scalar type (see CwiseBinaryOp constructor),
+  // we still want to handle the case when the result type is different.
+  typedef typename result_of<
+                     BinaryOp(
+                       const typename Lhs::Scalar&,
+                       const typename Rhs::Scalar&
+                     )
+                   >::type Scalar;
+  typedef typename cwise_promote_storage_type<typename traits<Lhs>::StorageKind,
+                                              typename traits<Rhs>::StorageKind,
+                                              BinaryOp>::ret StorageKind;
+  typedef typename promote_index_type<typename traits<Lhs>::StorageIndex,
+                                      typename traits<Rhs>::StorageIndex>::type StorageIndex;
+  typedef typename Lhs::Nested LhsNested;
+  typedef typename Rhs::Nested RhsNested;
+  typedef typename remove_reference<LhsNested>::type _LhsNested;
+  typedef typename remove_reference<RhsNested>::type _RhsNested;
+  enum {
+    Flags = cwise_promote_storage_order<typename traits<Lhs>::StorageKind,typename traits<Rhs>::StorageKind,_LhsNested::Flags & RowMajorBit,_RhsNested::Flags & RowMajorBit>::value
+  };
+};
+} // end namespace internal
+
+template<typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
+class CwiseBinaryOpImpl;
+
+/** \class CwiseBinaryOp
+  * \ingroup Core_Module
+  *
+  * \brief Generic expression where a coefficient-wise binary operator is applied to two expressions
+  *
+  * \tparam BinaryOp template functor implementing the operator
+  * \tparam LhsType the type of the left-hand side
+  * \tparam RhsType the type of the right-hand side
+  *
+  * This class represents an expression  where a coefficient-wise binary operator is applied to two expressions.
+  * It is the return type of binary operators, by which we mean only those binary operators where
+  * both the left-hand side and the right-hand side are Eigen expressions.
+  * For example, the return type of matrix1+matrix2 is a CwiseBinaryOp.
+  *
+  * Most of the time, this is the only way that it is used, so you typically don't have to name
+  * CwiseBinaryOp types explicitly.
+  *
+  * \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp
+  */
+template<typename BinaryOp, typename LhsType, typename RhsType>
+class CwiseBinaryOp : 
+  public CwiseBinaryOpImpl<
+          BinaryOp, LhsType, RhsType,
+          typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
+                                                        typename internal::traits<RhsType>::StorageKind,
+                                                        BinaryOp>::ret>,
+  internal::no_assignment_operator
+{
+  public:
+    
+    typedef typename internal::remove_all<BinaryOp>::type Functor;
+    typedef typename internal::remove_all<LhsType>::type Lhs;
+    typedef typename internal::remove_all<RhsType>::type Rhs;
+
+    typedef typename CwiseBinaryOpImpl<
+        BinaryOp, LhsType, RhsType,
+        typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
+                                                      typename internal::traits<Rhs>::StorageKind,
+                                                      BinaryOp>::ret>::Base Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp)
+
+    typedef typename internal::ref_selector<LhsType>::type LhsNested;
+    typedef typename internal::ref_selector<RhsType>::type RhsNested;
+    typedef typename internal::remove_reference<LhsNested>::type _LhsNested;
+    typedef typename internal::remove_reference<RhsNested>::type _RhsNested;
+
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp())
+      : m_lhs(aLhs), m_rhs(aRhs), m_functor(func)
+    {
+      EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp,typename Lhs::Scalar,typename Rhs::Scalar);
+      // require the sizes to match
+      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
+      eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols());
+    }
+
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Index rows() const {
+      // return the fixed size type if available to enable compile time optimizations
+      if (internal::traits<typename internal::remove_all<LhsNested>::type>::RowsAtCompileTime==Dynamic)
+        return m_rhs.rows();
+      else
+        return m_lhs.rows();
+    }
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Index cols() const {
+      // return the fixed size type if available to enable compile time optimizations
+      if (internal::traits<typename internal::remove_all<LhsNested>::type>::ColsAtCompileTime==Dynamic)
+        return m_rhs.cols();
+      else
+        return m_lhs.cols();
+    }
+
+    /** \returns the left hand side nested expression */
+    EIGEN_DEVICE_FUNC
+    const _LhsNested& lhs() const { return m_lhs; }
+    /** \returns the right hand side nested expression */
+    EIGEN_DEVICE_FUNC
+    const _RhsNested& rhs() const { return m_rhs; }
+    /** \returns the functor representing the binary operation */
+    EIGEN_DEVICE_FUNC
+    const BinaryOp& functor() const { return m_functor; }
+
+  protected:
+    LhsNested m_lhs;
+    RhsNested m_rhs;
+    const BinaryOp m_functor;
+};
+
+// Generic API dispatcher
+template<typename BinaryOp, typename Lhs, typename Rhs, typename StorageKind>
+class CwiseBinaryOpImpl
+  : public internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >::type Base;
+};
+
+/** replaces \c *this by \c *this - \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived> &other)
+{
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this + \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_CWISE_BINARY_OP_H

examples/ThirdPartyLibs/Eigen/src/Core/CwiseNullaryOp.h

@@ -0,0 +1,902 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_NULLARY_OP_H
+#define EIGEN_CWISE_NULLARY_OP_H
+
+namespace Eigen {
+
+namespace internal {
+template<typename NullaryOp, typename PlainObjectType>
+struct traits<CwiseNullaryOp<NullaryOp, PlainObjectType> > : traits<PlainObjectType>
+{
+  enum {
+    Flags = traits<PlainObjectType>::Flags & RowMajorBit
+  };
+};
+
+} // namespace internal
+
+/** \class CwiseNullaryOp
+  * \ingroup Core_Module
+  *
+  * \brief Generic expression of a matrix where all coefficients are defined by a functor
+  *
+  * \tparam NullaryOp template functor implementing the operator
+  * \tparam PlainObjectType the underlying plain matrix/array type
+  *
+  * This class represents an expression of a generic nullary operator.
+  * It is the return type of the Ones(), Zero(), Constant(), Identity() and Random() methods,
+  * and most of the time this is the only way it is used.
+  *
+  * However, if you want to write a function returning such an expression, you
+  * will need to use this class.
+  *
+  * The functor NullaryOp must expose one of the following method:
+    <table class="manual">
+    <tr            ><td>\c operator()() </td><td>if the procedural generation does not depend on the coefficient entries (e.g., random numbers)</td></tr>
+    <tr class="alt"><td>\c operator()(Index i)</td><td>if the procedural generation makes sense for vectors only and that it depends on the coefficient index \c i (e.g., linspace) </td></tr>
+    <tr            ><td>\c operator()(Index i,Index j)</td><td>if the procedural generation depends on the matrix coordinates \c i, \c j (e.g., to generate a checkerboard with 0 and 1)</td></tr>
+    </table>
+  * It is also possible to expose the last two operators if the generation makes sense for matrices but can be optimized for vectors.
+  *
+  * See DenseBase::NullaryExpr(Index,const CustomNullaryOp&) for an example binding
+  * C++11 random number generators.
+  *
+  * A nullary expression can also be used to implement custom sophisticated matrix manipulations
+  * that cannot be covered by the existing set of natively supported matrix manipulations.
+  * See this \ref TopicCustomizing_NullaryExpr "page" for some examples and additional explanations
+  * on the behavior of CwiseNullaryOp.
+  *
+  * \sa class CwiseUnaryOp, class CwiseBinaryOp, DenseBase::NullaryExpr
+  */
+template<typename NullaryOp, typename PlainObjectType>
+class CwiseNullaryOp : public internal::dense_xpr_base< CwiseNullaryOp<NullaryOp, PlainObjectType> >::type, internal::no_assignment_operator
+{
+  public:
+
+    typedef typename internal::dense_xpr_base<CwiseNullaryOp>::type Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(CwiseNullaryOp)
+
+    EIGEN_DEVICE_FUNC
+    CwiseNullaryOp(Index rows, Index cols, const NullaryOp& func = NullaryOp())
+      : m_rows(rows), m_cols(cols), m_functor(func)
+    {
+      eigen_assert(rows >= 0
+            && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
+            &&  cols >= 0
+            && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
+    }
+
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Index rows() const { return m_rows.value(); }
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Index cols() const { return m_cols.value(); }
+
+    /** \returns the functor representing the nullary operation */
+    EIGEN_DEVICE_FUNC
+    const NullaryOp& functor() const { return m_functor; }
+
+  protected:
+    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
+    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
+    const NullaryOp m_functor;
+};
+
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+DenseBase<Derived>::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func)
+{
+  return CwiseNullaryOp<CustomNullaryOp, PlainObject>(rows, cols, func);
+}
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * Here is an example with C++11 random generators: \include random_cpp11.cpp
+  * Output: \verbinclude random_cpp11.out
+  * 
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+DenseBase<Derived>::NullaryExpr(Index size, const CustomNullaryOp& func)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  if(RowsAtCompileTime == 1) return CwiseNullaryOp<CustomNullaryOp, PlainObject>(1, size, func);
+  else return CwiseNullaryOp<CustomNullaryOp, PlainObject>(size, 1, func);
+}
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+DenseBase<Derived>::NullaryExpr(const CustomNullaryOp& func)
+{
+  return CwiseNullaryOp<CustomNullaryOp, PlainObject>(RowsAtCompileTime, ColsAtCompileTime, func);
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this DenseBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Constant(Index rows, Index cols, const Scalar& value)
+{
+  return DenseBase<Derived>::NullaryExpr(rows, cols, internal::scalar_constant_op<Scalar>(value));
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this DenseBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Constant(Index size, const Scalar& value)
+{
+  return DenseBase<Derived>::NullaryExpr(size, internal::scalar_constant_op<Scalar>(value));
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * This variant is only for fixed-size DenseBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Constant(const Scalar& value)
+{
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return DenseBase<Derived>::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op<Scalar>(value));
+}
+
+/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&)
+  *
+  * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar,PacketScalar>(low,high,size));
+}
+
+/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&)
+  *
+  * \sa LinSpaced(Scalar,Scalar)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar,PacketScalar>(low,high,Derived::SizeAtCompileTime));
+}
+
+/**
+  * \brief Sets a linearly spaced vector.
+  *
+  * The function generates 'size' equally spaced values in the closed interval [low,high].
+  * When size is set to 1, a vector of length 1 containing 'high' is returned.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include DenseBase_LinSpaced.cpp
+  * Output: \verbinclude DenseBase_LinSpaced.out
+  *
+  * For integer scalar types, an even spacing is possible if and only if the length of the range,
+  * i.e., \c high-low is a scalar multiple of \c size-1, or if \c size is a scalar multiple of the
+  * number of values \c high-low+1 (meaning each value can be repeated the same number of time).
+  * If one of these two considions is not satisfied, then \c high is lowered to the largest value
+  * satisfying one of this constraint.
+  * Here are some examples:
+  *
+  * Example: \include DenseBase_LinSpacedInt.cpp
+  * Output: \verbinclude DenseBase_LinSpacedInt.out
+  *
+  * \sa setLinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(Index size, const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar,PacketScalar>(low,high,size));
+}
+
+/**
+  * \copydoc DenseBase::LinSpaced(Index, const Scalar&, const Scalar&)
+  * Special version for fixed size types which does not require the size parameter.
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+DenseBase<Derived>::LinSpaced(const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar,PacketScalar>(low,high,Derived::SizeAtCompileTime));
+}
+
+/** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */
+template<typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApproxToConstant
+(const Scalar& val, const RealScalar& prec) const
+{
+  typename internal::nested_eval<Derived,1>::type self(derived());
+  for(Index j = 0; j < cols(); ++j)
+    for(Index i = 0; i < rows(); ++i)
+      if(!internal::isApprox(self.coeff(i, j), val, prec))
+        return false;
+  return true;
+}
+
+/** This is just an alias for isApproxToConstant().
+  *
+  * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
+template<typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isConstant
+(const Scalar& val, const RealScalar& prec) const
+{
+  return isApproxToConstant(val, prec);
+}
+
+/** Alias for setConstant(): sets all coefficients in this expression to \a val.
+  *
+  * \sa setConstant(), Constant(), class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void DenseBase<Derived>::fill(const Scalar& val)
+{
+  setConstant(val);
+}
+
+/** Sets all coefficients in this expression to value \a val.
+  *
+  * \sa fill(), setConstant(Index,const Scalar&), setConstant(Index,Index,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setConstant(const Scalar& val)
+{
+  return derived() = Constant(rows(), cols(), val);
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to the given value \a val.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setConstant_int.cpp
+  * Output: \verbinclude Matrix_setConstant_int.out
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setConstant(Index size, const Scalar& val)
+{
+  resize(size);
+  return setConstant(val);
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to the given value \a val.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  * \param val the value to which all coefficients are set
+  *
+  * Example: \include Matrix_setConstant_int_int.cpp
+  * Output: \verbinclude Matrix_setConstant_int_int.out
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setConstant(Index rows, Index cols, const Scalar& val)
+{
+  resize(rows, cols);
+  return setConstant(val);
+}
+
+/**
+  * \brief Sets a linearly spaced vector.
+  *
+  * The function generates 'size' equally spaced values in the closed interval [low,high].
+  * When size is set to 1, a vector of length 1 containing 'high' is returned.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include DenseBase_setLinSpaced.cpp
+  * Output: \verbinclude DenseBase_setLinSpaced.out
+  *
+  * For integer scalar types, do not miss the explanations on the definition
+  * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
+  *
+  * \sa LinSpaced(Index,const Scalar&,const Scalar&), CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(Index newSize, const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op<Scalar,PacketScalar>(low,high,newSize));
+}
+
+/**
+  * \brief Sets a linearly spaced vector.
+  *
+  * The function fills \c *this with equally spaced values in the closed interval [low,high].
+  * When size is set to 1, a vector of length 1 containing 'high' is returned.
+  *
+  * \only_for_vectors
+  *
+  * For integer scalar types, do not miss the explanations on the definition
+  * of \link LinSpaced(Index,const Scalar&,const Scalar&) even spacing \endlink.
+  *
+  * \sa LinSpaced(Index,const Scalar&,const Scalar&), setLinSpaced(Index, const Scalar&, const Scalar&), CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(const Scalar& low, const Scalar& high)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return setLinSpaced(size(), low, high);
+}
+
+// zero:
+
+/** \returns an expression of a zero matrix.
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_zero_int_int.cpp
+  * Output: \verbinclude MatrixBase_zero_int_int.out
+  *
+  * \sa Zero(), Zero(Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Zero(Index rows, Index cols)
+{
+  return Constant(rows, cols, Scalar(0));
+}
+
+/** \returns an expression of a zero vector.
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_zero_int.cpp
+  * Output: \verbinclude MatrixBase_zero_int.out
+  *
+  * \sa Zero(), Zero(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Zero(Index size)
+{
+  return Constant(size, Scalar(0));
+}
+
+/** \returns an expression of a fixed-size zero matrix or vector.
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * Example: \include MatrixBase_zero.cpp
+  * Output: \verbinclude MatrixBase_zero.out
+  *
+  * \sa Zero(Index), Zero(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Zero()
+{
+  return Constant(Scalar(0));
+}
+
+/** \returns true if *this is approximately equal to the zero matrix,
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isZero.cpp
+  * Output: \verbinclude MatrixBase_isZero.out
+  *
+  * \sa class CwiseNullaryOp, Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isZero(const RealScalar& prec) const
+{
+  typename internal::nested_eval<Derived,1>::type self(derived());
+  for(Index j = 0; j < cols(); ++j)
+    for(Index i = 0; i < rows(); ++i)
+      if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<Scalar>(1), prec))
+        return false;
+  return true;
+}
+
+/** Sets all coefficients in this expression to zero.
+  *
+  * Example: \include MatrixBase_setZero.cpp
+  * Output: \verbinclude MatrixBase_setZero.out
+  *
+  * \sa class CwiseNullaryOp, Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setZero()
+{
+  return setConstant(Scalar(0));
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to zero.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setZero_int.cpp
+  * Output: \verbinclude Matrix_setZero_int.out
+  *
+  * \sa DenseBase::setZero(), setZero(Index,Index), class CwiseNullaryOp, DenseBase::Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setZero(Index newSize)
+{
+  resize(newSize);
+  return setConstant(Scalar(0));
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to zero.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setZero_int_int.cpp
+  * Output: \verbinclude Matrix_setZero_int_int.out
+  *
+  * \sa DenseBase::setZero(), setZero(Index), class CwiseNullaryOp, DenseBase::Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setZero(Index rows, Index cols)
+{
+  resize(rows, cols);
+  return setConstant(Scalar(0));
+}
+
+// ones:
+
+/** \returns an expression of a matrix where all coefficients equal one.
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_ones_int_int.cpp
+  * Output: \verbinclude MatrixBase_ones_int_int.out
+  *
+  * \sa Ones(), Ones(Index), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Ones(Index rows, Index cols)
+{
+  return Constant(rows, cols, Scalar(1));
+}
+
+/** \returns an expression of a vector where all coefficients equal one.
+  *
+  * The parameter \a newSize is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Ones() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_ones_int.cpp
+  * Output: \verbinclude MatrixBase_ones_int.out
+  *
+  * \sa Ones(), Ones(Index,Index), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Ones(Index newSize)
+{
+  return Constant(newSize, Scalar(1));
+}
+
+/** \returns an expression of a fixed-size matrix or vector where all coefficients equal one.
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * Example: \include MatrixBase_ones.cpp
+  * Output: \verbinclude MatrixBase_ones.out
+  *
+  * \sa Ones(Index), Ones(Index,Index), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+DenseBase<Derived>::Ones()
+{
+  return Constant(Scalar(1));
+}
+
+/** \returns true if *this is approximately equal to the matrix where all coefficients
+  *          are equal to 1, within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isOnes.cpp
+  * Output: \verbinclude MatrixBase_isOnes.out
+  *
+  * \sa class CwiseNullaryOp, Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isOnes
+(const RealScalar& prec) const
+{
+  return isApproxToConstant(Scalar(1), prec);
+}
+
+/** Sets all coefficients in this expression to one.
+  *
+  * Example: \include MatrixBase_setOnes.cpp
+  * Output: \verbinclude MatrixBase_setOnes.out
+  *
+  * \sa class CwiseNullaryOp, Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setOnes()
+{
+  return setConstant(Scalar(1));
+}
+
+/** Resizes to the given \a newSize, and sets all coefficients in this expression to one.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setOnes_int.cpp
+  * Output: \verbinclude Matrix_setOnes_int.out
+  *
+  * \sa MatrixBase::setOnes(), setOnes(Index,Index), class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setOnes(Index newSize)
+{
+  resize(newSize);
+  return setConstant(Scalar(1));
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to one.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setOnes_int_int.cpp
+  * Output: \verbinclude Matrix_setOnes_int_int.out
+  *
+  * \sa MatrixBase::setOnes(), setOnes(Index), class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setOnes(Index rows, Index cols)
+{
+  resize(rows, cols);
+  return setConstant(Scalar(1));
+}
+
+// Identity:
+
+/** \returns an expression of the identity matrix (not necessarily square).
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_identity_int_int.cpp
+  * Output: \verbinclude MatrixBase_identity_int_int.out
+  *
+  * \sa Identity(), setIdentity(), isIdentity()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
+MatrixBase<Derived>::Identity(Index rows, Index cols)
+{
+  return DenseBase<Derived>::NullaryExpr(rows, cols, internal::scalar_identity_op<Scalar>());
+}
+
+/** \returns an expression of the identity matrix (not necessarily square).
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variant taking size arguments.
+  *
+  * Example: \include MatrixBase_identity.cpp
+  * Output: \verbinclude MatrixBase_identity.out
+  *
+  * \sa Identity(Index,Index), setIdentity(), isIdentity()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
+MatrixBase<Derived>::Identity()
+{
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return MatrixBase<Derived>::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_identity_op<Scalar>());
+}
+
+/** \returns true if *this is approximately equal to the identity matrix
+  *          (not necessarily square),
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isIdentity.cpp
+  * Output: \verbinclude MatrixBase_isIdentity.out
+  *
+  * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), setIdentity()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isIdentity
+(const RealScalar& prec) const
+{
+  typename internal::nested_eval<Derived,1>::type self(derived());
+  for(Index j = 0; j < cols(); ++j)
+  {
+    for(Index i = 0; i < rows(); ++i)
+    {
+      if(i == j)
+      {
+        if(!internal::isApprox(self.coeff(i, j), static_cast<Scalar>(1), prec))
+          return false;
+      }
+      else
+      {
+        if(!internal::isMuchSmallerThan(self.coeff(i, j), static_cast<RealScalar>(1), prec))
+          return false;
+      }
+    }
+  }
+  return true;
+}
+
+namespace internal {
+
+template<typename Derived, bool Big = (Derived::SizeAtCompileTime>=16)>
+struct setIdentity_impl
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
+  {
+    return m = Derived::Identity(m.rows(), m.cols());
+  }
+};
+
+template<typename Derived>
+struct setIdentity_impl<Derived, true>
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
+  {
+    m.setZero();
+    const Index size = numext::mini(m.rows(), m.cols());
+    for(Index i = 0; i < size; ++i) m.coeffRef(i,i) = typename Derived::Scalar(1);
+    return m;
+  }
+};
+
+} // end namespace internal
+
+/** Writes the identity expression (not necessarily square) into *this.
+  *
+  * Example: \include MatrixBase_setIdentity.cpp
+  * Output: \verbinclude MatrixBase_setIdentity.out
+  *
+  * \sa class CwiseNullaryOp, Identity(), Identity(Index,Index), isIdentity()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity()
+{
+  return internal::setIdentity_impl<Derived>::run(derived());
+}
+
+/** \brief Resizes to the given size, and writes the identity expression (not necessarily square) into *this.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setIdentity_int_int.cpp
+  * Output: \verbinclude Matrix_setIdentity_int_int.out
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity(Index rows, Index cols)
+{
+  derived().resize(rows, cols);
+  return setIdentity();
+}
+
+/** \returns an expression of the i-th unit (basis) vector.
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(Index newSize, Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return BasisReturnType(SquareMatrixType::Identity(newSize,newSize), i);
+}
+
+/** \returns an expression of the i-th unit (basis) vector.
+  *
+  * \only_for_vectors
+  *
+  * This variant is for fixed-size vector only.
+  *
+  * \sa MatrixBase::Unit(Index,Index), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return BasisReturnType(SquareMatrixType::Identity(),i);
+}
+
+/** \returns an expression of the X axis unit vector (1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX()
+{ return Derived::Unit(0); }
+
+/** \returns an expression of the Y axis unit vector (0,1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY()
+{ return Derived::Unit(1); }
+
+/** \returns an expression of the Z axis unit vector (0,0,1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ()
+{ return Derived::Unit(2); }
+
+/** \returns an expression of the W axis unit vector (0,0,0,1)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(Index,Index), MatrixBase::Unit(Index), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW()
+{ return Derived::Unit(3); }
+
+/** \brief Set the coefficients of \c *this to the i-th unit (basis) vector
+  *
+  * \param i index of the unique coefficient to be set to 1
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  eigen_assert(i<size());
+  derived().setZero();
+  derived().coeffRef(i) = Scalar(1);
+  return derived();
+}
+
+/** \brief Resizes to the given \a newSize, and writes the i-th unit (basis) vector into *this.
+  *
+  * \param newSize the new size of the vector
+  * \param i index of the unique coefficient to be set to 1
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index newSize, Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  eigen_assert(i<newSize);
+  derived().resize(newSize);
+  return setUnit(i);
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_CWISE_NULLARY_OP_H

examples/ThirdPartyLibs/Eigen/src/Core/CwiseTernaryOp.h

@@ -0,0 +1,197 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2016 Eugene Brevdo <ebrevdo@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_TERNARY_OP_H
+#define EIGEN_CWISE_TERNARY_OP_H
+
+namespace Eigen {
+
+namespace internal {
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3>
+struct traits<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > {
+  // we must not inherit from traits<Arg1> since it has
+  // the potential to cause problems with MSVC
+  typedef typename remove_all<Arg1>::type Ancestor;
+  typedef typename traits<Ancestor>::XprKind XprKind;
+  enum {
+    RowsAtCompileTime = traits<Ancestor>::RowsAtCompileTime,
+    ColsAtCompileTime = traits<Ancestor>::ColsAtCompileTime,
+    MaxRowsAtCompileTime = traits<Ancestor>::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = traits<Ancestor>::MaxColsAtCompileTime
+  };
+
+  // even though we require Arg1, Arg2, and Arg3 to have the same scalar type
+  // (see CwiseTernaryOp constructor),
+  // we still want to handle the case when the result type is different.
+  typedef typename result_of<TernaryOp(
+      const typename Arg1::Scalar&, const typename Arg2::Scalar&,
+      const typename Arg3::Scalar&)>::type Scalar;
+
+  typedef typename internal::traits<Arg1>::StorageKind StorageKind;
+  typedef typename internal::traits<Arg1>::StorageIndex StorageIndex;
+
+  typedef typename Arg1::Nested Arg1Nested;
+  typedef typename Arg2::Nested Arg2Nested;
+  typedef typename Arg3::Nested Arg3Nested;
+  typedef typename remove_reference<Arg1Nested>::type _Arg1Nested;
+  typedef typename remove_reference<Arg2Nested>::type _Arg2Nested;
+  typedef typename remove_reference<Arg3Nested>::type _Arg3Nested;
+  enum { Flags = _Arg1Nested::Flags & RowMajorBit };
+};
+}  // end namespace internal
+
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3,
+          typename StorageKind>
+class CwiseTernaryOpImpl;
+
+/** \class CwiseTernaryOp
+  * \ingroup Core_Module
+  *
+  * \brief Generic expression where a coefficient-wise ternary operator is
+ * applied to two expressions
+  *
+  * \tparam TernaryOp template functor implementing the operator
+  * \tparam Arg1Type the type of the first argument
+  * \tparam Arg2Type the type of the second argument
+  * \tparam Arg3Type the type of the third argument
+  *
+  * This class represents an expression where a coefficient-wise ternary
+ * operator is applied to three expressions.
+  * It is the return type of ternary operators, by which we mean only those
+ * ternary operators where
+  * all three arguments are Eigen expressions.
+  * For example, the return type of betainc(matrix1, matrix2, matrix3) is a
+ * CwiseTernaryOp.
+  *
+  * Most of the time, this is the only way that it is used, so you typically
+ * don't have to name
+  * CwiseTernaryOp types explicitly.
+  *
+  * \sa MatrixBase::ternaryExpr(const MatrixBase<Argument2> &, const
+ * MatrixBase<Argument3> &, const CustomTernaryOp &) const, class CwiseBinaryOp,
+ * class CwiseUnaryOp, class CwiseNullaryOp
+  */
+template <typename TernaryOp, typename Arg1Type, typename Arg2Type,
+          typename Arg3Type>
+class CwiseTernaryOp : public CwiseTernaryOpImpl<
+                           TernaryOp, Arg1Type, Arg2Type, Arg3Type,
+                           typename internal::traits<Arg1Type>::StorageKind>,
+                       internal::no_assignment_operator
+{
+ public:
+  typedef typename internal::remove_all<Arg1Type>::type Arg1;
+  typedef typename internal::remove_all<Arg2Type>::type Arg2;
+  typedef typename internal::remove_all<Arg3Type>::type Arg3;
+
+  typedef typename CwiseTernaryOpImpl<
+      TernaryOp, Arg1Type, Arg2Type, Arg3Type,
+      typename internal::traits<Arg1Type>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseTernaryOp)
+
+  typedef typename internal::ref_selector<Arg1Type>::type Arg1Nested;
+  typedef typename internal::ref_selector<Arg2Type>::type Arg2Nested;
+  typedef typename internal::ref_selector<Arg3Type>::type Arg3Nested;
+  typedef typename internal::remove_reference<Arg1Nested>::type _Arg1Nested;
+  typedef typename internal::remove_reference<Arg2Nested>::type _Arg2Nested;
+  typedef typename internal::remove_reference<Arg3Nested>::type _Arg3Nested;
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE CwiseTernaryOp(const Arg1& a1, const Arg2& a2,
+                                     const Arg3& a3,
+                                     const TernaryOp& func = TernaryOp())
+      : m_arg1(a1), m_arg2(a2), m_arg3(a3), m_functor(func) {
+    // require the sizes to match
+    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg2)
+    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Arg1, Arg3)
+
+    // The index types should match
+    EIGEN_STATIC_ASSERT((internal::is_same<
+                         typename internal::traits<Arg1Type>::StorageKind,
+                         typename internal::traits<Arg2Type>::StorageKind>::value),
+                        STORAGE_KIND_MUST_MATCH)
+    EIGEN_STATIC_ASSERT((internal::is_same<
+                         typename internal::traits<Arg1Type>::StorageKind,
+                         typename internal::traits<Arg3Type>::StorageKind>::value),
+                        STORAGE_KIND_MUST_MATCH)
+
+    eigen_assert(a1.rows() == a2.rows() && a1.cols() == a2.cols() &&
+                 a1.rows() == a3.rows() && a1.cols() == a3.cols());
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE Index rows() const {
+    // return the fixed size type if available to enable compile time
+    // optimizations
+    if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
+                RowsAtCompileTime == Dynamic &&
+        internal::traits<typename internal::remove_all<Arg2Nested>::type>::
+                RowsAtCompileTime == Dynamic)
+      return m_arg3.rows();
+    else if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
+                     RowsAtCompileTime == Dynamic &&
+             internal::traits<typename internal::remove_all<Arg3Nested>::type>::
+                     RowsAtCompileTime == Dynamic)
+      return m_arg2.rows();
+    else
+      return m_arg1.rows();
+  }
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE Index cols() const {
+    // return the fixed size type if available to enable compile time
+    // optimizations
+    if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
+                ColsAtCompileTime == Dynamic &&
+        internal::traits<typename internal::remove_all<Arg2Nested>::type>::
+                ColsAtCompileTime == Dynamic)
+      return m_arg3.cols();
+    else if (internal::traits<typename internal::remove_all<Arg1Nested>::type>::
+                     ColsAtCompileTime == Dynamic &&
+             internal::traits<typename internal::remove_all<Arg3Nested>::type>::
+                     ColsAtCompileTime == Dynamic)
+      return m_arg2.cols();
+    else
+      return m_arg1.cols();
+  }
+
+  /** \returns the first argument nested expression */
+  EIGEN_DEVICE_FUNC
+  const _Arg1Nested& arg1() const { return m_arg1; }
+  /** \returns the first argument nested expression */
+  EIGEN_DEVICE_FUNC
+  const _Arg2Nested& arg2() const { return m_arg2; }
+  /** \returns the third argument nested expression */
+  EIGEN_DEVICE_FUNC
+  const _Arg3Nested& arg3() const { return m_arg3; }
+  /** \returns the functor representing the ternary operation */
+  EIGEN_DEVICE_FUNC
+  const TernaryOp& functor() const { return m_functor; }
+
+ protected:
+  Arg1Nested m_arg1;
+  Arg2Nested m_arg2;
+  Arg3Nested m_arg3;
+  const TernaryOp m_functor;
+};
+
+// Generic API dispatcher
+template <typename TernaryOp, typename Arg1, typename Arg2, typename Arg3,
+          typename StorageKind>
+class CwiseTernaryOpImpl
+    : public internal::generic_xpr_base<
+          CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >::type {
+ public:
+  typedef typename internal::generic_xpr_base<
+      CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >::type Base;
+};
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_CWISE_TERNARY_OP_H

examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryOp.h

@@ -0,0 +1,103 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_UNARY_OP_H
+#define EIGEN_CWISE_UNARY_OP_H
+
+namespace Eigen { 
+
+namespace internal {
+template<typename UnaryOp, typename XprType>
+struct traits<CwiseUnaryOp<UnaryOp, XprType> >
+ : traits<XprType>
+{
+  typedef typename result_of<
+                     UnaryOp(const typename XprType::Scalar&)
+                   >::type Scalar;
+  typedef typename XprType::Nested XprTypeNested;
+  typedef typename remove_reference<XprTypeNested>::type _XprTypeNested;
+  enum {
+    Flags = _XprTypeNested::Flags & RowMajorBit 
+  };
+};
+}
+
+template<typename UnaryOp, typename XprType, typename StorageKind>
+class CwiseUnaryOpImpl;
+
+/** \class CwiseUnaryOp
+  * \ingroup Core_Module
+  *
+  * \brief Generic expression where a coefficient-wise unary operator is applied to an expression
+  *
+  * \tparam UnaryOp template functor implementing the operator
+  * \tparam XprType the type of the expression to which we are applying the unary operator
+  *
+  * This class represents an expression where a unary operator is applied to an expression.
+  * It is the return type of all operations taking exactly 1 input expression, regardless of the
+  * presence of other inputs such as scalars. For example, the operator* in the expression 3*matrix
+  * is considered unary, because only the right-hand side is an expression, and its
+  * return type is a specialization of CwiseUnaryOp.
+  *
+  * Most of the time, this is the only way that it is used, so you typically don't have to name
+  * CwiseUnaryOp types explicitly.
+  *
+  * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp
+  */
+template<typename UnaryOp, typename XprType>
+class CwiseUnaryOp : public CwiseUnaryOpImpl<UnaryOp, XprType, typename internal::traits<XprType>::StorageKind>, internal::no_assignment_operator
+{
+  public:
+
+    typedef typename CwiseUnaryOpImpl<UnaryOp, XprType,typename internal::traits<XprType>::StorageKind>::Base Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp)
+    typedef typename internal::ref_selector<XprType>::type XprTypeNested;
+    typedef typename internal::remove_all<XprType>::type NestedExpression;
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp())
+      : m_xpr(xpr), m_functor(func) {}
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Index rows() const { return m_xpr.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Index cols() const { return m_xpr.cols(); }
+
+    /** \returns the functor representing the unary operation */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const UnaryOp& functor() const { return m_functor; }
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const typename internal::remove_all<XprTypeNested>::type&
+    nestedExpression() const { return m_xpr; }
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    typename internal::remove_all<XprTypeNested>::type&
+    nestedExpression() { return m_xpr; }
+
+  protected:
+    XprTypeNested m_xpr;
+    const UnaryOp m_functor;
+};
+
+// Generic API dispatcher
+template<typename UnaryOp, typename XprType, typename StorageKind>
+class CwiseUnaryOpImpl
+  : public internal::generic_xpr_base<CwiseUnaryOp<UnaryOp, XprType> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<CwiseUnaryOp<UnaryOp, XprType> >::type Base;
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CWISE_UNARY_OP_H

examples/ThirdPartyLibs/Eigen/src/Core/CwiseUnaryView.h

@@ -0,0 +1,128 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_UNARY_VIEW_H
+#define EIGEN_CWISE_UNARY_VIEW_H
+
+namespace Eigen {
+
+namespace internal {
+template<typename ViewOp, typename MatrixType>
+struct traits<CwiseUnaryView<ViewOp, MatrixType> >
+ : traits<MatrixType>
+{
+  typedef typename result_of<
+                     ViewOp(const typename traits<MatrixType>::Scalar&)
+                   >::type Scalar;
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
+    Flags = traits<_MatrixTypeNested>::Flags & (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions
+    MatrixTypeInnerStride =  inner_stride_at_compile_time<MatrixType>::ret,
+    // need to cast the sizeof's from size_t to int explicitly, otherwise:
+    // "error: no integral type can represent all of the enumerator values
+    InnerStrideAtCompileTime = MatrixTypeInnerStride == Dynamic
+                             ? int(Dynamic)
+                             : int(MatrixTypeInnerStride) * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)),
+    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret == Dynamic
+                             ? int(Dynamic)
+                             : outer_stride_at_compile_time<MatrixType>::ret * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar))
+  };
+};
+}
+
+template<typename ViewOp, typename MatrixType, typename StorageKind>
+class CwiseUnaryViewImpl;
+
+/** \class CwiseUnaryView
+  * \ingroup Core_Module
+  *
+  * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector
+  *
+  * \tparam ViewOp template functor implementing the view
+  * \tparam MatrixType the type of the matrix we are applying the unary operator
+  *
+  * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector.
+  * It is the return type of real() and imag(), and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp
+  */
+template<typename ViewOp, typename MatrixType>
+class CwiseUnaryView : public CwiseUnaryViewImpl<ViewOp, MatrixType, typename internal::traits<MatrixType>::StorageKind>
+{
+  public:
+
+    typedef typename CwiseUnaryViewImpl<ViewOp, MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView)
+    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+
+    explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
+      : m_matrix(mat), m_functor(func) {}
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
+
+    EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); }
+    EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); }
+
+    /** \returns the functor representing unary operation */
+    const ViewOp& functor() const { return m_functor; }
+
+    /** \returns the nested expression */
+    const typename internal::remove_all<MatrixTypeNested>::type&
+    nestedExpression() const { return m_matrix; }
+
+    /** \returns the nested expression */
+    typename internal::remove_reference<MatrixTypeNested>::type&
+    nestedExpression() { return m_matrix.const_cast_derived(); }
+
+  protected:
+    MatrixTypeNested m_matrix;
+    ViewOp m_functor;
+};
+
+// Generic API dispatcher
+template<typename ViewOp, typename XprType, typename StorageKind>
+class CwiseUnaryViewImpl
+  : public internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type Base;
+};
+
+template<typename ViewOp, typename MatrixType>
+class CwiseUnaryViewImpl<ViewOp,MatrixType,Dense>
+  : public internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type
+{
+  public:
+
+    typedef CwiseUnaryView<ViewOp, MatrixType> Derived;
+    typedef typename internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type Base;
+
+    EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
+    
+    EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
+
+    EIGEN_DEVICE_FUNC inline Index innerStride() const
+    {
+      return derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
+    }
+
+    EIGEN_DEVICE_FUNC inline Index outerStride() const
+    {
+      return derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
+    }
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CWISE_UNARY_VIEW_H