git-svn-id: svn://svn.icms.temple.edu/lammps-ro/trunk@24 f3b2605a-c512-4ea7-a41b-209d697bcdaa

lib/README

@@ -0,0 +1,12 @@
+This directory contains libraries that can be linked with when
+building LAMMPS.  The library itself must be built first, so that a
+lib*.a file exists for LAMMPS to link against.
+
+Each library directory contains a README with additional info.  You
+will need to copy one of the Makefile.* files to Makefile before
+building a library.  If a Makefile.* suitable for your machine does
+not exist, you will need to edit one of the existing Makefiles.
+
+The libraries included with LAMMPS are the following:
+
+poems	      POEMS rigid-body integration package from RPI

lib/poems/Authors_List.txt

@@ -0,0 +1,28 @@
+ /*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: AUTHORS LIST                                           *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2004 by Authors as listed in Author's List          *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *_________________________________________________________________________*/
+ 
+ 
+ 
+ POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE, contains
+ contributions from the following authors.
+  
+KURT S ANDERSON (anderk5@rpi.edu)
+RUDRANARAYAN M MUKHERJEE (mukher@rpi.edu)
+JAMES H CRITCHLEY 
+JOHN L ZIEGLER
+SCOTT LIPTON
+GEOFFREY BASORE
+JOHN EVANS

lib/poems/Copyright_Notice

@@ -0,0 +1,10 @@
+				   Copyright (C) 2006
+Authors as indicated in Author List and Rensselaer Polytechnic Institute
+
+Parallelizable Open source Efficient Multibody Software (POEMS) is written and maintained by the authors as indicated in Author's List as a part of Scientific Computation Research Center (SCOREC) at Rensselaer Polytechnic Intitute, Troy, NY, USA.
+
+This program is free software; you can redistribute it and/or modify it under the terms of the license as found in POEMS_License.txt distributed with the source code.
+
+This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the license text for more details.
+   
+A copy of the license file POEMS_License.txt, Authors List and Grants List files should be distributed along with this program. 

lib/poems/Grants_List.txt

@@ -0,0 +1,28 @@
+ /*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: GRANTS LIST                                            *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2004 by Authors as listed in Author's List          *
+ *      DOWNLOAD: www.rpi.edu/~anderk5/POEMS                               *
+ *_________________________________________________________________________*/
+ 
+ 
+ 
+ 
+ POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE, 
+ has been supported in part by the following grants. Their support is
+ highly appreciated.
+
+ NSF Nanoscale Interdisciplinary Research Team 0303902
+ NSF State Time 0219734
+ NSF Faculty Early Career Development (CAREER)  9733684
+ Sandia National Laboratories LDRD project 67017
+ 

lib/poems/Makefile

@@ -0,0 +1,101 @@
+# *
+# *_________________________________________________________________________*
+# *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+# *      DESCRIPTION: SEE READ-ME                                           *
+# *      FILE NAME: Makefile                                                *
+# *      AUTHORS: See Author List                                           * 
+# *      GRANTS: See Grants List                                            *
+# *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+# *      LICENSE: Please see License Agreement                              *
+# *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+# *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+# *                     Computational Dynamics Lab                          *
+# *                     Rensselaer Polytechnic Institute                    *
+# *                     110 8th St. Troy NY 12180                           * 
+# *      CONTACT:        anderk5@rpi.edu                                    *
+# *_________________________________________________________________________*/
+
+
+
+SHELL = /bin/sh
+
+
+# ------ FILES ------
+
+SRC_MAIN = workspace.cpp system.cpp poemsobject.cpp 
+INC_MAIN = workspace.h system.h poemsobject.h
+
+SRC_BODY =      body.cpp rigidbody.cpp particle.cpp inertialframe.cpp
+INC_BODY =      bodies.h body.h rigidbody.h particle.h inertialframe.h
+
+
+SRC_JOINT =     joint.cpp revolutejoint.cpp prismaticjoint.cpp sphericaljoint.cpp \
+    freebodyjoint.cpp body23joint.cpp mixedjoint.cpp
+INC_JOINT =     joints.h joint.h revolutejoint.h prismaticjoint.h sphericaljoint.h \
+    freebodyjoint.h body23joint.h mixedjoint.h
+
+SRC_POINT =     point.cpp fixedpoint.cpp
+INC_POINT =     points.h point.h fixedpoint.h
+
+SRC_SOLVE = solver.cpp  
+INC_SOLVE = solver.h  
+
+SRC_ORDERN = onsolver.cpp onfunctions.cpp onbody.cpp
+INC_ORDERN = onsolver.h onfunctions.h onbody.h
+
+SRC_MAT =       virtualmatrix.cpp matrix.cpp matrixfun.cpp mat3x3.cpp virtualcolmatrix.cpp \
+    colmatrix.cpp vect3.cpp virtualrowmatrix.cpp rowmatrix.cpp mat6x6.cpp vect6.cpp \
+    fastmatrixops.cpp colmatmap.cpp eulerparameters.cpp vect4.cpp norm.cpp mat4x4.cpp \
+    
+INC_MAT =       matrices.h virtualmatrix.h matrix.h matrixfun.h mat3x3.h virtualcolmatrix.h \
+    colmatrix.h vect3.h virtualrowmatrix.h rowmatrix.h mat6x6.h vect6.h \
+    fastmatrixops.h colmatmap.h eulerparameters.h vect4.h norm.h mat4x4.h 
+
+SRC_MISC = poemstreenode.cpp
+INC_MISC = poemslist.h poemstreenode.h poemstree.h poemsnodelib.h SystemProcessor.h defines.h POEMSChain.h
+
+SRC = $(SRC_MAIN) $(SRC_BODY) $(SRC_JOINT) $(SRC_POINT) $(SRC_SOLVE) $(SRC_ORDERN) $(SRC_MAT) $(SRC_MISC) 
+INC = $(INC_MAIN) $(INC_BODY) $(INC_JOINT) $(INC_POINT) $(INC_SOLVE) $(INC_ORDERN) $(INC_MAT) $(INC_MISC) 
+
+FILES = $(SRC) $(INC) Makefile Authors_List.txt Grants_List.txt POEMS_License.txt README Copyright_Notice
+
+# ------ DEFINITIONS ------
+
+LIB = libpoems.a
+OBJ =   $(SRC:.cpp=.o)
+
+# ------ SETTINGS ------
+
+CC =	        g++
+CCFLAGS =       -O -g -Wall #-Wno-deprecated
+ARCHIVE =	ar
+ARCHFLAG =	-rc
+DEPFLAGS =      -M
+LINK =         	g++
+LINKFLAGS =	-O
+USRLIB =
+SYSLIB =
+
+# ------ MAKE PROCEDURE ------
+
+lib: 	$(OBJ)
+	$(ARCHIVE) $(ARFLAGS) $(LIB) $(OBJ)
+
+# ------ COMPILE RULES ------
+
+%.o:%.cpp
+	$(CC) $(CCFLAGS) -c $<
+%.d:%.cpp
+	$(CC) $(CCFLAGS) $(DEPFLAGS) $< > $@		
+
+# ------ DEPENDENCIES ------
+
+DEPENDS = $(OBJ:.o=.d)
+
+# ------ CLEAN ------
+
+clean:
+	-rm *.o *.d *~ $(LIB)
+
+tar:
+	-tar -cvf ../POEMS.tar $(FILES)

lib/poems/Makefile.icc

@@ -0,0 +1,101 @@
+# *
+# *_________________________________________________________________________*
+# *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+# *      DESCRIPTION: SEE READ-ME                                           *
+# *      FILE NAME: Makefile                                                *
+# *      AUTHORS: See Author List                                           * 
+# *      GRANTS: See Grants List                                            *
+# *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+# *      LICENSE: Please see License Agreement                              *
+# *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+# *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+# *                     Computational Dynamics Lab                          *
+# *                     Rensselaer Polytechnic Institute                    *
+# *                     110 8th St. Troy NY 12180                           * 
+# *      CONTACT:        anderk5@rpi.edu                                    *
+# *_________________________________________________________________________*/
+
+
+
+SHELL = /bin/sh
+
+
+# ------ FILES ------
+
+SRC_MAIN = workspace.cpp system.cpp poemsobject.cpp 
+INC_MAIN = workspace.h system.h poemsobject.h
+
+SRC_BODY =      body.cpp rigidbody.cpp particle.cpp inertialframe.cpp
+INC_BODY =      bodies.h body.h rigidbody.h particle.h inertialframe.h
+
+
+SRC_JOINT =     joint.cpp revolutejoint.cpp prismaticjoint.cpp sphericaljoint.cpp \
+    freebodyjoint.cpp body23joint.cpp mixedjoint.cpp
+INC_JOINT =     joints.h joint.h revolutejoint.h prismaticjoint.h sphericaljoint.h \
+    freebodyjoint.h body23joint.h mixedjoint.h
+
+SRC_POINT =     point.cpp fixedpoint.cpp
+INC_POINT =     points.h point.h fixedpoint.h
+
+SRC_SOLVE = solver.cpp  
+INC_SOLVE = solver.h  
+
+SRC_ORDERN = onsolver.cpp onfunctions.cpp onbody.cpp
+INC_ORDERN = onsolver.h onfunctions.h onbody.h
+
+SRC_MAT =       virtualmatrix.cpp matrix.cpp matrixfun.cpp mat3x3.cpp virtualcolmatrix.cpp \
+    colmatrix.cpp vect3.cpp virtualrowmatrix.cpp rowmatrix.cpp mat6x6.cpp vect6.cpp \
+    fastmatrixops.cpp colmatmap.cpp eulerparameters.cpp vect4.cpp norm.cpp mat4x4.cpp \
+    
+INC_MAT =       matrices.h virtualmatrix.h matrix.h matrixfun.h mat3x3.h virtualcolmatrix.h \
+    colmatrix.h vect3.h virtualrowmatrix.h rowmatrix.h mat6x6.h vect6.h \
+    fastmatrixops.h colmatmap.h eulerparameters.h vect4.h norm.h mat4x4.h 
+
+SRC_MISC = poemstreenode.cpp
+INC_MISC = poemslist.h poemstreenode.h poemstree.h poemsnodelib.h SystemProcessor.h defines.h POEMSChain.h
+
+SRC = $(SRC_MAIN) $(SRC_BODY) $(SRC_JOINT) $(SRC_POINT) $(SRC_SOLVE) $(SRC_ORDERN) $(SRC_MAT) $(SRC_MISC) 
+INC = $(INC_MAIN) $(INC_BODY) $(INC_JOINT) $(INC_POINT) $(INC_SOLVE) $(INC_ORDERN) $(INC_MAT) $(INC_MISC) 
+
+FILES = $(SRC) $(INC) Makefile Authors_List.txt Grants_List.txt POEMS_License.txt README Copyright_Notice
+
+# ------ DEFINITIONS ------
+
+LIB = libpoems.a
+OBJ =   $(SRC:.cpp=.o)
+
+# ------ SETTINGS ------
+
+CC =		icc
+CCFLAGS =       -O -Wall -Wcheck -wd869,981,1572
+ARCHIVE =	ar
+ARCHFLAG =	-rc
+DEPFLAGS =      -M
+LINK =		icc
+LINKFLAGS =	-O
+USRLIB =
+SYSLIB =
+
+# ------ MAKE PROCEDURE ------
+
+lib: 	$(OBJ)
+	$(ARCHIVE) $(ARFLAGS) $(LIB) $(OBJ)
+
+# ------ COMPILE RULES ------
+
+%.o:%.cpp
+	$(CC) $(CCFLAGS) -c $<
+%.d:%.cpp
+	$(CC) $(CCFLAGS) $(DEPFLAGS) $< > $@		
+
+# ------ DEPENDENCIES ------
+
+DEPENDS = $(OBJ:.o=.d)
+
+# ------ CLEAN ------
+
+clean:
+	-rm *.o *.d *~ $(LIB)
+
+tar:	
+	-tar -cvf ../POEMS.tar $(FILES)

lib/poems/Makefile.storm

@@ -0,0 +1,101 @@
+# *
+# *_________________________________________________________________________*
+# *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+# *      DESCRIPTION: SEE READ-ME                                           *
+# *      FILE NAME: Makefile                                                *
+# *      AUTHORS: See Author List                                           * 
+# *      GRANTS: See Grants List                                            *
+# *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+# *      LICENSE: Please see License Agreement                              *
+# *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+# *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+# *                     Computational Dynamics Lab                          *
+# *                     Rensselaer Polytechnic Institute                    *
+# *                     110 8th St. Troy NY 12180                           * 
+# *      CONTACT:        anderk5@rpi.edu                                    *
+# *_________________________________________________________________________*/
+
+
+
+SHELL = /bin/sh
+
+
+# ------ FILES ------
+
+SRC_MAIN = workspace.cpp system.cpp poemsobject.cpp 
+INC_MAIN = workspace.h system.h poemsobject.h
+
+SRC_BODY =      body.cpp rigidbody.cpp particle.cpp inertialframe.cpp
+INC_BODY =      bodies.h body.h rigidbody.h particle.h inertialframe.h
+
+
+SRC_JOINT =     joint.cpp revolutejoint.cpp prismaticjoint.cpp sphericaljoint.cpp \
+    freebodyjoint.cpp body23joint.cpp mixedjoint.cpp
+INC_JOINT =     joints.h joint.h revolutejoint.h prismaticjoint.h sphericaljoint.h \
+    freebodyjoint.h body23joint.h mixedjoint.h
+
+SRC_POINT =     point.cpp fixedpoint.cpp
+INC_POINT =     points.h point.h fixedpoint.h
+
+SRC_SOLVE = solver.cpp  
+INC_SOLVE = solver.h  
+
+SRC_ORDERN = onsolver.cpp onfunctions.cpp onbody.cpp
+INC_ORDERN = onsolver.h onfunctions.h onbody.h
+
+SRC_MAT =       virtualmatrix.cpp matrix.cpp matrixfun.cpp mat3x3.cpp virtualcolmatrix.cpp \
+    colmatrix.cpp vect3.cpp virtualrowmatrix.cpp rowmatrix.cpp mat6x6.cpp vect6.cpp \
+    fastmatrixops.cpp colmatmap.cpp eulerparameters.cpp vect4.cpp norm.cpp mat4x4.cpp \
+    
+INC_MAT =       matrices.h virtualmatrix.h matrix.h matrixfun.h mat3x3.h virtualcolmatrix.h \
+    colmatrix.h vect3.h virtualrowmatrix.h rowmatrix.h mat6x6.h vect6.h \
+    fastmatrixops.h colmatmap.h eulerparameters.h vect4.h norm.h mat4x4.h 
+
+SRC_MISC = poemstreenode.cpp
+INC_MISC = poemslist.h poemstreenode.h poemstree.h poemsnodelib.h SystemProcessor.h defines.h POEMSChain.h
+
+SRC = $(SRC_MAIN) $(SRC_BODY) $(SRC_JOINT) $(SRC_POINT) $(SRC_SOLVE) $(SRC_ORDERN) $(SRC_MAT) $(SRC_MISC) 
+INC = $(INC_MAIN) $(INC_BODY) $(INC_JOINT) $(INC_POINT) $(INC_SOLVE) $(INC_ORDERN) $(INC_MAT) $(INC_MISC) 
+
+FILES = $(SRC) $(INC) Makefile Authors_List.txt Grants_List.txt POEMS_License.txt README Copyright_Notice
+
+# ------ DEFINITIONS ------
+
+LIB = libpoems.a
+OBJ =   $(SRC:.cpp=.o)
+
+# ------ SETTINGS ------
+
+CC =	        CC
+CCFLAGS =       -O -g -Wall #-Wno-deprecated
+ARCHIVE =	ar
+ARCHFLAG =	-rc
+DEPFLAGS =      -M
+LINK =         	CC
+LINKFLAGS =	-O
+USRLIB =
+SYSLIB =
+
+# ------ MAKE PROCEDURE ------
+
+lib: 	$(OBJ)
+	$(ARCHIVE) $(ARFLAGS) $(LIB) $(OBJ)
+
+# ------ COMPILE RULES ------
+
+%.o:%.cpp
+	$(CC) $(CCFLAGS) -c $<
+%.d:%.cpp
+	$(CC) $(CCFLAGS) $(DEPFLAGS) $< > $@		
+
+# ------ DEPENDENCIES ------
+
+DEPENDS = $(OBJ:.o=.d)
+
+# ------ CLEAN ------
+
+clean:
+	-rm *.o *.d *~ $(LIB)
+
+tar:
+	-tar -cvf ../POEMS.tar $(FILES)

lib/poems/POEMSChain.h

@@ -0,0 +1,73 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: PoemsChain.h                                            *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef POEMSCHAIN_H_
+#define POEMSCHAIN_H_
+
+#include "poemslist.h"
+
+struct ChildRingData {
+	List<int> * childRing;
+	int entranceNodeId;
+};
+
+struct POEMSChain{
+	~POEMSChain(){
+		for(int i = 0; i < childChains.GetNumElements(); i++)
+		{
+			delete childChains(i);
+		}
+	}
+	//void printTreeStructure(int tabs);
+	//void getTreeAsList(List<int> * temp);
+	List<int> listOfNodes;
+	List<POEMSChain> childChains;
+	POEMSChain * parentChain;
+	List<ChildRingData> childRings;
+	
+	
+	void printTreeStructure(int tabs){
+		for(int i = 0; i < tabs; i++)
+		{
+			cout << "\t";
+		}
+		cout << "Chain: ";
+		for(int i = 0; i < listOfNodes.GetNumElements(); i++)
+		{
+			cout << *(listOfNodes(i)) << " ";
+		}
+		cout << endl;
+		for(int i = 0; i < childChains.GetNumElements(); i++)
+		{
+			childChains(i)->printTreeStructure(tabs + 1);
+		}
+	}
+	void getTreeAsList(List<int> * temp)
+	{
+		for(int i = 0; i < listOfNodes.GetNumElements(); i++)
+		{
+			int * integer = new int;
+			*integer = *(listOfNodes(i));
+			temp->Append(integer);
+		}
+		for(int i = 0; i < childChains.GetNumElements(); i++)
+		{
+			childChains(i)->getTreeAsList(temp);
+		}
+	}
+};
+#endif

lib/poems/POEMS_License.txt

@@ -0,0 +1,64 @@
+   Copyright (C) 2005
+   Authors as indicated in Author List and Rensselaer Polytechnic Institute
+
+   Parallelizable Open source Efficient Multibody Software (POEMS) is written and maintained by the
+   authors as indicated in Author's List as a part of Scientific Computation Research Center (SCOREC)
+   at Rensselaer Polytechnic Intitute, Troy, NY, USA.
+   
+   This License Agreement applies to any software library or other program which contains a notice
+   placed by the copyright holder saying it may be distributed under the terms of this Rensselaer 
+   Public License (also called "this License"). Each licensee is addressed as "you".
+
+   A "library" means a collection of software functions and/or data prepared so as to be conveniently
+   linked with application programs (which use some of those functions and data) to form executables.
+   The "Library", below, refers to any such software library or work which has been distributed under
+   these terms.  A "work based on the Library" means either the Library/code or any derivative work
+   under copyright law.
+
+   "Source code" for a work means the preferred form of the work for making modifications to it including
+   any associated interface definition files, and scripts used to control compilation  and installation
+   of the library.
+
+   This library is free software; Redistribution and use in source code and binary forms, with or
+   without modification, are permitted provided that the following conditions are met: 
+   
+1. Redistributions of source code must retain the above copyright notice, this list of conditions, 
+   the authors list, the grants list, and the following disclaimer without any modifications and in
+   its current form. 
+   
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions,
+   the authors list, the grants list, and the following disclaimer in the documentation and/or other
+   materials provided with the distribution. 
+   
+3. You must cause the files modified to carry prominent notices stating that you changed the files
+   and dates of the change.  
+   
+4. At your discretion, you may submit the differences between the program and your work based on the 
+   program for inclusion in the source code repository maintained by Rensselaer Polytechnic Institute.
+   The mechanics of the submission are explained in the file named CONTRIBUTE distributed with the
+   Library. Should you decide to submit the changes, you agree to provide Rensselaer Polytechnic
+   Institute with a joint copyright assignment of the changes, and you agree to sign such a copyright
+   assignment form at the request of Rensselaer Polytechnic Institute. Changes will then made as 
+   appropriate to the authors list at the Resselaer repository. 
+   
+5. The authors as listed in "Authors_List" reserve the right to distribute the software or any modified
+   work in any other license form if they so choose to without infringing on any previous or concurrent
+   license agreement.   
+       
+6. The name of the authors or their employer(s), Rensselaer Polytechnic Institute, the Computational
+   Dynamics Laboratory,  the Scientific Computation Research Center (SCoReC), nor the names of any
+   of the software distributors may not be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+
+
+THIS SOFTWARE IS PROVIDED BY THE AUTHORS "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 AUTHORS OR THE SCIENTIFIC COMPUTATION RESEARCH CENTER 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. 
+
+ 

lib/poems/README

@@ -0,0 +1,28 @@
+ POEMS 1.0 README
+
+
+The Parallelizable Open source Efficient Multibody Software (POEMS) is general purpose distributed multibody dynamics software, which is able to simulate the dynamics of articulated body systems.
+
+This work is supported by the funding agencies listed in the Grants' List. POEMS is an open source program distributed under the Rensselaer Scorec License.
+
+The Authors as listed in Authors' List reserve the right to reject the request on technical supports of the POEMS freely obtained.
+
+
+We are open to hear from you about bugs, an idea for improvement, and suggestions, etc. We keep improving the POEMS. Check the POEMS web site (www.rpi.edu/~anderk5/POEMS) for the recent changes.
+
+
+All correspondence regarding the POEMS should be sent to:
+
+By email: (preferred)
+Prof. Kurt Anderson (anderk5@rpi.edu) or Rudranarayan Mukherjee (mukher@rpi.edu) - include "[POEMS]" in the subject
+
+or by mail:
+Prof. Kurt S. Anderson
+4006 Jonsson Engineering Center 	
+Rensselaer Polytechnic Institute
+110 8th Street,
+Troy, NY 12180-3510, U.S.A.
+
+	
+
+Created: June 6, 2006

lib/poems/SystemProcessor.h

@@ -0,0 +1,283 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: SystemProcessor.h                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef _SYS_PROCESSOR_H_
+#define _SYS_PROCESSOR_H_
+#include "poemslist.h"
+#include "poemstree.h"
+#include "POEMSChain.h"
+
+
+struct POEMSNode {
+	List<POEMSNode> links;
+	List<bool> taken;
+	int idNumber;
+	bool visited;
+	
+	~POEMSNode(){
+		for(int i = 0; i < taken.GetNumElements(); i++)
+		{
+			delete taken(i);
+		}
+	};	
+};
+
+
+class SystemProcessor{
+private:
+	Tree nodes;
+//	List<POEMSNode> forDeletion;
+	List<POEMSChain> headsOfSystems;
+	List<List<int> > ringsInSystem;
+	POEMSNode * findSingleLink(TreeNode * aNode);
+	POEMSChain * AddNewChain(POEMSNode * currentNode);
+	bool setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode);
+public:
+	SystemProcessor(void);
+	
+	~SystemProcessor(void)	{
+		headsOfSystems.DeleteValues();
+		for(int i = 0; i < ringsInSystem.GetNumElements(); i++)
+		{
+			for(int k = 0; k < ringsInSystem(i)->GetNumElements(); i++)
+			{
+				delete (*ringsInSystem(i))(k);
+			}
+		}
+	};
+	void processArray(int** links, int numLinks);
+	List<POEMSChain> * getSystemData();
+	int getNumberOfHeadChains();
+};
+
+SystemProcessor::SystemProcessor(void){
+}
+
+void SystemProcessor::processArray(int** links, int numLinks)
+{
+	bool * false_var;					//holds the value false; needed because a constant cannot be put into a list; the list requires a
+										//reference.
+	for(int i = 0; i < numLinks; i++)	//go through all the links in the input array
+	{
+		if(!nodes.Find(links[i][0]))	//if the first node in the pair is not found in the storage tree
+		{
+			POEMSNode * newNode = new POEMSNode;	//make a new node
+//			forDeletion.Append(newNode);
+			newNode->idNumber = links[i][0];		//set its ID to the value
+			newNode->visited = false;				//set it to be unvisited
+			nodes.Insert(links[i][0], links[i][0], (void *) newNode);	//and add it to the tree storage structure
+		}
+		if(!nodes.Find(links[i][1]))				//repeat process for the other half of each link
+		{
+			POEMSNode * newNode = new POEMSNode;
+//			forDeletion.Append(newNode);
+			newNode->idNumber = links[i][1];
+			newNode->visited = false;
+			nodes.Insert(links[i][1], links[i][1], (void *) newNode);
+		}
+		POEMSNode * firstNode = (POEMSNode *)nodes.Find(links[i][0]);	//now that we are sure both nodes exist,
+		POEMSNode * secondNode = (POEMSNode *)nodes.Find(links[i][1]);	//we can get both of them out of the tree
+		firstNode->links.Append(secondNode);							//and add the link from the first to the second...
+		false_var = new bool;
+		*false_var = false;												//make a new false boolean to note that the link between these two
+		firstNode->taken.Append(false_var);								//has not already been taken, and append it to the taken list
+		secondNode->links.Append(firstNode);							//repeat process for link from second node to first
+		false_var = new bool;
+		*false_var = false;
+		secondNode->taken.Append(false_var);
+	}	
+	
+	TreeNode * temp = nodes.GetRoot();							//get the root node of the node storage tree
+	POEMSNode * currentNode;
+	do
+	{
+		currentNode = findSingleLink(temp);						//find the start of the next available chain
+		if(currentNode != NULL)
+		{
+			headsOfSystems.Append(AddNewChain(currentNode));							//and add it to the headsOfSystems list of chains
+		}
+	} 
+	while(currentNode != NULL);									//repeat this until all chains have been added		
+}
+
+POEMSChain * SystemProcessor::AddNewChain(POEMSNode * currentNode){
+	if(currentNode == NULL)	//Termination condition; if the currentNode is null, then return null
+	{
+		return NULL;
+	}
+	int * tmp;
+	POEMSNode * nextNode = NULL;	//nextNode stores the proposed next node to add to the chain.  this will be checked to make sure no backtracking is occuring before being assigned as the current node.
+	POEMSChain * newChain = new POEMSChain;	//make a new POEMSChain object.  This will be the object returned
+
+	if(currentNode->links.GetNumElements() == 0)	//if we have no links from this node, then the whole chain is only one node.  Add this node to the chain and return it; mark node as visited for future reference
+	{
+		currentNode->visited = true;
+		tmp = new int;
+		*tmp = currentNode->idNumber;
+		newChain->listOfNodes.Append(tmp);
+		return newChain;
+	}
+	while(currentNode->links.GetNumElements() <= 2)	//we go until we get to a node that branches, or both branches have already been taken both branches can already be taken if a loop with no spurs is found in the input data
+	{
+		currentNode->visited = true;
+		tmp = new int;
+		*tmp = currentNode->idNumber;
+		newChain->listOfNodes.Append(tmp);	//append the current node to the chain & mark as visited
+		//cout << "Appending node " << currentNode->idNumber << " to chain" << endl;
+		nextNode = currentNode->links.GetHeadElement()->value;	//the next node is the first or second value stored in the links array
+																//of the current node.  We get the first value...
+		if(!setLinkVisited(currentNode, nextNode))					//...and see if it points back to where we came from. If it does...
+		{														//either way, we set this link as visited
+			if(currentNode->links.GetNumElements() == 1)		//if it does, then if that is the only link to this node, we're done with the chain, so append the chain to the list and return the newly created chain
+			{
+//				headsOfSystems.Append(newChain);
+				return newChain;
+			}
+			nextNode = currentNode->links.GetHeadElement()->next->value;//follow the other link if there is one, so we go down the chain
+			if(!setLinkVisited(currentNode, nextNode))				//mark link as followed, so we know not to backtrack			
+			{
+	//			headsOfSystems.Append(newChain);
+				return newChain;								//This condition, where no branches have occurred but both links have already
+																//been taken can only occur in a loop with no spurs; add this loop to the
+																//system (currently added as a chain for consistency), and return.
+			}
+		}
+		currentNode = nextNode;									//set the current node to be the next node in the chain
+	}
+	currentNode->visited = true;
+	tmp = new int;
+	*tmp = currentNode->idNumber;
+	newChain->listOfNodes.Append(tmp);		//append the last node before branch (node shared jointly with branch chains)
+																//re-mark as visited, just to make sure
+	ListElement<POEMSNode> * tempNode = currentNode->links.GetHeadElement();	//go through all of the links, one at a time that branch
+	POEMSChain * tempChain = NULL;								//temporary variable to hold data 
+	while(tempNode != NULL)										//when we have followed all links, stop
+	{
+		if(setLinkVisited(tempNode->value, currentNode))		//dont backtrack, or create closed loops
+		{
+			tempChain = AddNewChain(tempNode->value);			//Add a new chain created out of the next node down that link
+			tempChain->parentChain = newChain;					//set the parent to be this chain
+			newChain->childChains.Append(tempChain);			//append the chain to this chain's list of child chains
+		}
+		tempNode = tempNode->next;								//go to process the next chain
+	}
+	//headsOfSystems.Append(newChain);							//append this chain to the system list		
+	return newChain;	
+}
+
+POEMSNode * SystemProcessor::findSingleLink(TreeNode * aNode)
+//This function takes the root of a search tree containing POEMSNodes and returns a POEMSNode corresponding to the start of a chain in the
+//system.  It finds a node that has not been visited before, and only has one link; this node will be used as the head of the chain.
+{
+	if(aNode == NULL)	
+	{
+		return NULL;
+	}
+	POEMSNode * returnVal =  (POEMSNode *)aNode->GetAuxData();	//get the poemsnode data out of the treenode
+	POEMSNode * detectLoneLoops = NULL;							//is used to handle a loop that has no protruding chains
+	if(returnVal->visited == false)
+	{
+		detectLoneLoops = returnVal;							//if we find any node that has not been visited yet, save it
+	}
+	if(returnVal->links.GetNumElements() == 1 && returnVal->visited == false)	//see if it has one element and hasnt been visited already
+	{
+		return returnVal;														//return the node is it meets this criteria
+	}
+	returnVal = findSingleLink(aNode->Left());									//otherwise, check the left subtree
+	if(returnVal == NULL)														//and if we find nothing...
+	{
+		returnVal = findSingleLink(aNode->Right());								//check the right subtree
+	}
+	if(returnVal == NULL)														//if we could not find any chains
+	{
+		returnVal = detectLoneLoops;											//see if we found any nodes at all that havent been processed
+	}
+	return returnVal;															//return what we find (will be NULL if no new chains are 
+																				//found)
+}
+
+bool SystemProcessor::setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode)
+//setLinkVisited sets the links between these two nodes as visited. If they are already visited, it returns false.  Otherwise, it sets
+//them as visited and returns true.  This function is used to see whether a certain path has been taken already in the graph structure.
+//If it has been, then we need to know so we dont follow it again; this prevents infinite recursion when there is a loop, and prevents
+//backtracking up a chain that has already been made.  The list of booleans denoting if a link has been visited is called 'taken' and is
+//part of the POEMSNode struct.  The list is the same size as the list of pointers to other nodes, and stores the boolean visited/unvisited
+//value for that particular link.  Because each link is represented twice, (once at each node in the link), both of the boolean values need
+//to be set in the event that the link has to be set as visited.
+{
+	//cout << "Checking link between nodes " << firstNode->idNumber << " and " << secondNode->idNumber << "... ";
+	ListElement<POEMSNode> * tmp = firstNode->links.GetHeadElement();	//get the head element of the list of pointers for node 1
+	ListElement<bool> * tmp2 = firstNode->taken.GetHeadElement();		//get the head element of the list of bool isVisited flags for node 1
+	while(tmp->value != NULL || tmp2->value != NULL)					//go through untill we reach the end of the lists
+	{
+		if(tmp->value == secondNode)							//if we find the link to the other node
+		{
+			if(*(tmp2->value) == true)							//if the link has already been visited
+			{
+				 //cout << "visited already" << endl;
+				return false;									//return false to indicate that the link has been visited before this attempt
+			}
+			else												//otherwise, visit it
+			{
+				*tmp2->value = true;
+			}
+			break;
+		}
+		tmp = tmp->next;										//go check next link
+		tmp2 = tmp2->next;
+	}
+
+	tmp = secondNode->links.GetHeadElement();			//now, if the link was unvisited, we need to go set the other node's list such that
+														//it also knows this link is being visited
+	tmp2 = secondNode->taken.GetHeadElement();
+	while(tmp->value != NULL || tmp2->value != NULL)	//go through the list
+	{
+		if(tmp->value == firstNode)						//if we find the link
+		{
+			if(*(tmp2->value) == true)					//and it has already been visited, then signal an error; this shouldnt ever happen
+			{
+				cout << "Error in parsing structure! Should never reach this condition! \n" << 
+						"Record of visited links out of synch between two adjacent nodes.\n";
+				return false;
+			}
+			else
+			{
+				*tmp2->value = true;					//set the appropriate value to true to indicate this link has been visited
+			}
+			break;
+		}
+		tmp = tmp->next;
+		tmp2 = tmp2->next;
+	}
+	//cout << "not visited" << endl;
+	return true;										//return true to indicate that this is the first time the link has been visited
+}
+
+List<POEMSChain> * SystemProcessor::getSystemData(void)	//Gets the list of POEMSChains that comprise the system.  Might eventually only
+														//return chains linked to the reference plane, but currently returns every chain
+														//in the system.
+{
+	return &headsOfSystems;
+}
+
+int SystemProcessor::getNumberOfHeadChains(void) //This function isnt implemented yet, and might be taken out entirely; this was a holdover
+												//from when I intended to return an array of chain pointers, rather than a list of chains
+												//It will probably be deleted once I finish figuring out exactly what needs to be returned
+{
+	return 0;
+}
+#endif

lib/poems/bodies.h

@@ -0,0 +1,27 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: bodies.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef BODIES_H
+#define BODIES_H
+
+#include "body.h"
+#include "inertialframe.h"
+#include "rigidbody.h"
+#include "particle.h"
+
+#endif

lib/poems/body.cpp

@@ -0,0 +1,137 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: body.cpp                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+#include "bodies.h"
+#include "point.h"
+
+using namespace std;
+
+Body::Body()
+{
+  inertia.Zeros();
+  mass = 0;
+  a_t.Zeros();
+}
+
+Body::~Body()
+{
+  points.DeleteValues();
+}
+
+bool Body::ReadIn(istream& in){
+  return (ReadInBodyData(in) && ReadInPoints(in));
+}
+
+void Body::WriteOut(ostream& out){
+  // write out header <type> <name>
+  out << GetType() << ' ' << GetName() << endl;
+
+  // write out body specific data
+  WriteOutBodyData(out);
+
+  // write out points
+  WriteOutPoints(out);
+
+}
+
+bool Body::ReadInPoints(istream& in){
+  // get numfixed points
+  int numpoints;
+  int index;
+  Point* point;
+  int pointtype;
+  char pointname[256];
+
+  in >> numpoints;
+
+  for(int i=points.GetNumElements();i<numpoints;i++){
+    // error check
+    in >> index;
+    if(index != i){
+      cerr << "Invalid file format" << endl;
+      return false;
+    }
+
+    in >> pointtype >> pointname;
+    point = NewPoint(pointtype);
+
+    if(!point){
+      cerr << "Unrecognized point type '" << pointtype << endl;
+      return false;
+    }
+
+    // add the point
+    AddPoint(point);
+
+    // set generic point info
+    point->ChangeName(pointname);
+
+    // read in the rest of its data
+    if(!point->ReadIn(in)) return false;
+  }
+  return true;
+}
+
+void Body::WriteOutPoints(std::ostream& out){
+  int numpoints = points.GetNumElements();
+
+  out << numpoints << endl;
+
+  // list element pointer
+  ListElement<Point>* ele = points.GetHeadElement();
+
+  // set the ID of each point
+  for(int i=0;i<numpoints;i++){
+    ele->value->SetID(i);
+    out << i << ' ';
+    ele->value->WriteOut(out);
+
+    // increment list element pointer
+    ele = ele->next;
+  }
+  out << endl;
+}
+
+Point* Body::GetPoint(int p) {
+  return points(p);
+}
+
+void Body::AddJoint(Joint* joint){
+  joints.Append(joint);
+}
+
+void Body::AddPoint(Point* point){
+  points.Append(point);
+}
+
+//
+// global body functions
+//
+
+Body* NewBody(int type){
+  switch( BodyType(type) )
+    {
+      case INERTIALFRAME :  // The inertial reference frame
+        return new InertialFrame;
+      case RIGIDBODY :  // A Rigid Body
+        return new RigidBody;
+      case PARTICLE :  // A Particle
+        return new Particle;
+      default  :  // error
+        return 0;
+    }
+}

lib/poems/body.h

@@ -0,0 +1,81 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: body.h                                                  *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#ifndef BODY_H
+#define BODY_H
+
+#include "poemslist.h"
+#include <iostream>
+#include "poemsobject.h"
+
+#include "matrices.h"
+
+
+
+// emumerated type
+enum BodyType {
+  INERTIALFRAME = 0,
+  PARTICLE = 1,
+  RIGIDBODY = 2
+};
+  
+class Point;
+class Joint;
+class CompBody;
+
+class Body : public POEMSObject {
+public:
+  double mass;
+  Mat3x3 inertia;
+  
+  Vect3 r;
+  Vect3 v;
+  Vect3 v_k;
+  Vect3 a;
+  Vect3 a_t;
+  Mat3x3 n_C_k;
+  Vect3 omega;
+  Vect3 omega_k;
+  Vect3 alpha;
+  Vect3 alpha_t;
+  double KE;
+  
+
+  List<Joint> joints;
+  List<Point> points;
+
+  Body();
+  
+  bool ReadIn(std::istream& in);
+  void WriteOut(std::ostream& out);
+  bool ReadInPoints(std::istream& in);
+  void WriteOutPoints(std::ostream& out);
+  Point* GetPoint(int p);
+  void AddJoint(Joint* joint);
+  void AddPoint(Point* point);
+  
+  virtual bool ReadInBodyData(std::istream& in) = 0;
+  virtual void WriteOutBodyData(std::ostream& out) = 0;
+  virtual ~Body();
+  virtual BodyType GetType() = 0;
+};
+
+// global body functions
+Body* NewBody(int type);
+
+#endif

lib/poems/body23joint.cpp

@@ -0,0 +1,260 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: body23joint.cpp			                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#include "body23joint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+#include "norm.h"
+#include "eulerparameters.h"
+#include "matrices.h"
+#include <iomanip>
+
+
+Body23Joint::Body23Joint(){
+  DimQandU(4,2);
+}
+Body23Joint::~Body23Joint(){
+}
+
+JointType Body23Joint::GetType(){
+  return BODY23JOINT;
+}
+
+bool Body23Joint::ReadInJointData(std::istream& in){
+  return true;
+}
+
+void Body23Joint::WriteOutJointData(std::ostream& out){
+}
+
+Matrix Body23Joint::GetForward_sP(){
+	Vect3 temp = -(point2->position);  // This is vector from joint to CM
+	Matrix sP(6,2);
+	sP.Zeros();
+	
+	sP(2,1) =1.0;
+	sP(3,2) =1.0;	
+	sP(5,2) = temp(1);
+	sP(6,1) = -temp(1);	
+	return sP;    
+}
+
+void Body23Joint::UpdateForward_sP( Matrix& sP){
+    // sP is constant, do nothing.
+  // linear part is not constant
+}
+
+Matrix Body23Joint::GetBackward_sP(){
+	cout<<" -----------"<<endl;
+	cout<<"Am I here "<<endl;
+	cout<<" -----------"<<endl;	  
+	Vect3 temp = (point2->position);  // This is vector from CM to joint 
+	Matrix sP(6,2);
+	sP.Zeros();
+	sP(2,1) =1.0;
+	sP(3,2) =1.0;
+	
+	sP(5,2) = temp(1);
+	sP(6,1) = -temp(1);
+	
+	return sP;    
+}
+
+void Body23Joint::UpdateBackward_sP( Matrix& sP){  
+  // sP is constant, do nothing.
+}
+
+void Body23Joint::ComputeLocalTransform(){
+	Mat3x3 ko_C_k;
+	// Obtain the transformation matrix from euler parameters
+	EP_Transformation(q, ko_C_k);
+	FastMult(pk_C_ko,ko_C_k,pk_C_k);
+	}  
+
+
+void Body23Joint::ForwardKinematics(){
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 pk_w_k;
+  
+  
+  //cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
+  EP_Normalize(q);
+  
+
+  // orientations
+  ComputeForwardTransforms();    
+  
+  
+  //----------------------------------//
+  // COMPUTE POSITION VECTOR R12 aka GAMMA    
+  
+  FastNegMult(pk_C_k,point2->position,result1); // parents basis      
+  FastAdd(result1,point1->position,r12);      
+  
+  // compute position vector r21 
+  FastNegMult(k_C_pk,r12,r21); 
+
+  
+  //----------------------------------//
+  // COMPUTE GLOBAL LOCATION  
+  FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);    
+  FastAdd(result1,body1->r,result1);  
+  FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);  
+  FastAdd(result1,result2,body2->r);  
+    
+  ColMatrix temp_u(3);
+  qdot_to_u(q, temp_u, qdot);  
+  
+  temp_u(1)=0.0; 
+  u(1) = temp_u(2);
+  u(2) = temp_u(3);
+  
+
+  //-----------------------------------  
+  // angular velocities  
+  
+  FastAssign(temp_u,pk_w_k);
+  FastTMult(pk_C_k,body1->omega_k,result1);        
+  FastAdd(result1,pk_w_k,body2->omega_k);
+  FastMult(body2->n_C_k,body2->omega_k,body2->omega);  // June 1 checked with Lammps       
+  //-----------------------------------  
+  
+  // compute velocities  
+  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);  
+  FastAdd(body1->v_k,result1,result2);
+  FastTMult(pk_C_k,result2,result1); // In body basis  
+  
+  FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);                   
+  FastAdd(result1,result2,body2->v_k);    // In body basis   
+  FastMult(body2->n_C_k,body2->v_k,body2->v); 
+  
+  //------------------------------------------
+  //Compute the KE   
+  Matrix tempke;
+  tempke = T(body2->v_k)*(body2->v_k);    
+  double ke = 0.0;
+  ke = body2->mass*tempke(1,1);    
+  FastMult(body2->inertia,body2->omega_k,result1);
+  tempke= T(body2->omega_k)*result1;  
+  ke = 0.5*ke + 0.5*tempke(1,1);
+  body2->KE=ke;
+  
+  //-----------------------------------  
+  // compute state explicit angular acceleration  // Has to be in body basis    
+  FastTMult(pk_C_k,body1->alpha_t,result2);  
+  FastCross(body2->omega_k,pk_w_k,result1);
+  FastAdd(result1,result2,body2->alpha_t); 
+   
+  //-----------------------------------  
+  // compute state explicit acceleration   
+  // NEED TO DO THIS COMPLETELY IN BODY BASIS 
+  
+  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
+  FastCross(body1->omega_k,result1,result2); 
+  FastTMult(pk_C_k,result2,result1); 
+  
+  //FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);  
+  FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);  
+  FastCross(body2->omega_k,result3,result2);    
+  FastAdd(result1,result2,result3); //wxwxr in body basis  
+  
+  FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4); 
+    
+  FastTMult(pk_C_k,result4,result5);  
+  FastAssign(result5,result4);
+  
+  FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);   
+  FastAdd(result2,result4,result1); //alphaxr in body basis  
+  
+  FastTMult(pk_C_k,body1->a_t,result2);
+  FastTripleSum(result3,result1,result2,body2->a_t);     // in body basis  
+  
+  
+  //-----------------------------------    
+}
+
+// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
+void Body23Joint::BackwardKinematics(){
+  cout<<"what about here "<<endl;
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 k_w_pk;
+
+  // orientations
+  ComputeBackwardTransforms();
+  
+
+  // compute position vector r21
+  //r21 = point2->position - k_C_pk * point1->position;
+  FastMult(k_C_pk,point1->position,result1);
+  FastSubt(point2->position,result1,r21);
+  
+
+  // compute position vector r21
+  FastNegMult(pk_C_k,r21,r12);
+
+  // compute global location
+  // body1->r = body2->r + body2->n_C_k * r21;
+  FastMult(body2->n_C_k,r21,result1);
+  FastAdd(body2->r,result1,body1->r);
+
+  // compute qdot (for revolute joint qdot = u)
+  // qdot = u
+  ColMatrix us(3);
+  /*us(1)=0;
+  us(2)=u(1);
+  us(3)=u(2);*/
+  EP_Derivatives(q,u,qdot);
+
+  // angular velocities
+ 
+  FastMult(body2->n_C_k,u,result2);
+  FastAdd(body2->omega,result2,body1->omega);
+  FastAssign(u,k_w_pk);
+  FastMult(pk_C_k,body2->omega_k,result1);
+  FastSubt(result1,k_w_pk,body1->omega_k);
+  cout<<"The program was here"<<endl;
+
+  // compute velocities
+  FastCross(body2->omega_k,r21,result1);
+  FastCross(point1->position,k_w_pk,result2);
+  FastAdd(body2->v_k,result1,result3);
+  FastMult(pk_C_k,result3,result4);
+  FastAdd(result2,result4,body1->v_k);
+  FastMult(body1->n_C_k,body1->v_k,body1->v);
+
+  // compute state explicit angular acceleration
+  FastCross(body1->omega_k,k_w_pk,result1);
+  FastMult(pk_C_k,body2->alpha_t,result2);
+  FastAdd(result1,result2,body1->alpha_t);
+  
+  // compute state explicit acceleration
+  FastCross(body2->alpha_t,point2->position,result1);
+  FastCross(body2->omega_k,point2->position,result2);
+  FastCross(body2->omega_k,result2,result3);
+  FastTripleSum(body2->a_t,result1,result3,result4);
+  FastMult(pk_C_k,result4,result5);
+
+  FastCross(point1->position,body1->alpha_t,result1);
+  FastCross(point1->position,body1->omega_k,result2);
+  FastCross(body1->omega_k,result2,result3);
+
+  FastTripleSum(result5,result1,result3,body1->a_t);
+  
+}

lib/poems/body23joint.h

@@ -0,0 +1,44 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: body23joint.h			                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef BODY23JOINT_H
+#define BODY23JOINT_H
+
+#include "joint.h"
+#include "vect3.h"
+#include "mat3x3.h"
+
+
+
+class Body23Joint : public Joint  {
+  Matrix const_sP;
+public:
+  Body23Joint();
+  ~Body23Joint();
+  JointType GetType();
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ComputeLocalTransform();
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/colmatmap.cpp

@@ -0,0 +1,197 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: colmatmap.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "colmatmap.h"
+#include "colmatrix.h"
+#include <iostream>
+
+using namespace std;
+
+ColMatMap::ColMatMap(){
+	numrows = 0;
+	elements = 0;
+}
+
+ColMatMap::~ColMatMap(){
+	delete [] elements;
+}
+
+ColMatMap::ColMatMap(const ColMatMap& A){  // copy constructor
+	numrows = 0;
+	elements = 0;
+	Dim(A.numrows);
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.elements[i];
+}
+
+ColMatMap::ColMatMap(ColMatrix& A){  // copy constructor
+	numrows = 0;
+	elements = 0;
+	Dim(A.GetNumRows());
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.GetElementPointer(i);
+}
+
+/*
+ColMatrix::ColMatrix(const VirtualMatrix& A){  // copy constructor
+  if( A.GetNumCols() != 1 ){
+    cerr << "error trying to write a 2D matrix to a collumn" << endl;
+    exit(1);
+  }
+  numrows = 0;
+  elements = 0;
+  Dim(A.GetNumRows());
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+*/
+
+ColMatMap::ColMatMap(int m){  // size constructor
+	numrows = 0;
+	elements = 0;
+	Dim(m);
+}
+
+void ColMatMap::Dim(int m){
+	delete [] elements;
+	numrows = m;
+	elements = new double* [m];
+}
+
+void ColMatMap::Const(double value){
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = value;
+}
+
+MatrixType ColMatMap::GetType() const{
+	return COLMATMAP;
+}
+
+ostream& ColMatMap::WriteData(ostream& c) const{  //output
+	c << numrows << ' ';
+	for(int i=0;i<numrows;i++)
+		c << *(elements[i]) << ' ';
+	return c;
+}
+
+double& ColMatMap::operator_1int (int i){ // array access
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+	return *(elements[i-1]);
+}
+
+double ColMatMap::Get_1int(int i) const{
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index exceeded in Get" << endl;
+		exit(1);
+	}
+	return *(elements[i-1]);
+}
+
+void ColMatMap::Set_1int(int i, double value){
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index exceeded in Set" << endl;
+		exit(1);
+	}
+	*(elements[i-1]) = value;
+}
+
+double ColMatMap::BasicGet_1int(int i) const{
+	return *(elements[i]);
+}
+
+void ColMatMap::SetElementPointer(int i, double* p){
+	elements[i] = p;
+}
+
+double* ColMatMap::GetElementPointer(int i){
+	return elements[i];
+}
+
+void ColMatMap::BasicSet_1int(int i, double value){
+	*(elements[i]) = value;
+}
+
+void ColMatMap::BasicIncrement_1int(int i, double value){
+	*(elements[i]) += value;
+}
+
+void ColMatMap::AssignVM(const VirtualMatrix& A){
+	if(A.GetNumRows() != numrows){
+		cerr << "dimension mismatch in ColMatMap assignment" << endl;
+		exit(0);
+	}
+	if( A.GetNumCols() != 1 ){
+		cerr << "error trying to write a 2D matrix to a collumn" << endl;
+		exit(1);
+	}
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = A.BasicGet(i,0);
+}
+
+ColMatMap& ColMatMap::operator=(const ColMatMap& A){ // assignment operator
+	if(A.numrows != numrows){
+		cerr << "dimension mismatch in ColMatMap assignment" << endl;
+		exit(0);
+	}
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = *(A.elements[i]);
+	return *this;
+}
+
+ColMatMap& ColMatMap::operator=(const ColMatrix& A){ // assignment operator
+	if(A.GetNumRows() != numrows){
+		cerr << "dimension mismatch in ColMatMap assignment" << endl;
+		exit(0);
+	}
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = A.BasicGet(i);
+	return *this;
+}
+
+ColMatMap& ColMatMap::operator=(const VirtualColMatrix& A){ // overloaded =
+	if(A.GetNumRows() != numrows){
+		cerr << "dimension mismatch in ColMatMap assignment" << endl;
+		exit(0);
+	}
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = A.BasicGet(i);
+	return *this;
+}
+
+ColMatMap& ColMatMap::operator=(const VirtualMatrix& A){ // overloaded =
+	if(A.GetNumRows() != numrows){
+		cerr << "dimension mismatch in ColMatMap assignment" << endl;
+		exit(0);
+	}
+	if( A.GetNumCols() != 1 ){
+		cerr << "error trying to write a 2D matrix to a collumn" << endl;
+		exit(1);
+	}
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) = A.BasicGet(i,0);
+	return *this;
+}
+
+ColMatMap& ColMatMap::operator*=(double b){
+	for(int i=0;i<numrows;i++)
+		*(elements[i]) *= b;
+	return *this;
+}
+

lib/poems/colmatmap.h

@@ -0,0 +1,65 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: colmatmap.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef COLMATMAP_H
+#define COLMATMAP_H
+
+#include "virtualcolmatrix.h"
+
+class ColMatrix;
+
+class ColMatMap : public VirtualColMatrix  {
+        double** elements;
+public:
+        ColMatMap();
+        ~ColMatMap();
+        ColMatMap(const ColMatMap& A);  // copy constructor
+        ColMatMap(ColMatrix& A);  // copy constructor
+        ColMatMap(int m);  // size constructor
+
+        double& operator_1int (int i); // array access
+        double Get_1int(int i) const;
+        void Set_1int(int i, double value);
+        double BasicGet_1int(int i) const;
+        void BasicSet_1int(int i, double value);
+        void BasicIncrement_1int(int i, double value);
+        void SetElementPointer(int i, double* p);
+        double* GetElementPointer(int i);
+        void Dim(int m);
+        void Const(double value);
+        MatrixType GetType() const;
+        std::ostream& WriteData(std::ostream& c) const;
+
+        void AssignVM(const VirtualMatrix& A);
+        ColMatMap& operator=(const ColMatMap& A); // assignment operator
+        ColMatMap& operator=(const ColMatrix& A); // overloaded =
+        ColMatMap& operator=(const VirtualColMatrix& A); // overloaded =
+        ColMatMap& operator=(const VirtualMatrix& A); // overloaded =
+        ColMatMap& operator*=(double b);
+
+        // Fast Matrix Operators
+        friend void FastAssign(ColMatMap& A, ColMatMap& C); //C = A
+        friend void FastAssign(ColMatMap& A, ColMatrix& C);  // C = A
+        friend void FastAssign(ColMatrix& A, ColMatMap& C);  // C = A
+        friend void FastForwardEuler(ColMatMap& X,ColMatMap& Xdot, double dt);
+        friend void FastForwardEuler(ColMatMap& X,ColMatrix& Xdot, double dt);
+        friend void FastCKRK5(ColMatMap& X, ColMatrix& Xi, ColMatrix* f, double* c, double dt);
+        friend void FastFRK5(ColMatMap& X, ColMatrix& Xi, ColMatrix* f, double* c, double dt);
+};
+
+#endif

lib/poems/colmatrix.cpp

@@ -0,0 +1,204 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: colmatrix.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "colmatrix.h"
+#include "rowmatrix.h"
+#include <iostream>
+#include <cmath>
+
+using namespace std;
+
+ColMatrix::ColMatrix(){
+	numrows = 0;
+	elements = 0;
+}
+
+ColMatrix::~ColMatrix(){
+	delete [] elements;
+}
+
+ColMatrix::ColMatrix(const ColMatrix& A){  // copy constructor
+	numrows = 0;
+	elements = 0;
+	Dim(A.numrows);
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.elements[i];
+}
+
+ColMatrix::ColMatrix(const VirtualColMatrix& A){  // copy constructor
+	numrows = 0;
+	elements = 0;
+	Dim(A.GetNumRows());
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.BasicGet(i);
+}
+
+ColMatrix::ColMatrix(const VirtualMatrix& A){  // copy constructor
+	if( A.GetNumCols() != 1 ){
+		cerr << "error trying to write a 2D matrix to a collumn" << endl;
+		exit(1);
+	}
+	numrows = 0;
+	elements = 0;
+	Dim(A.GetNumRows());
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.BasicGet(i,0);
+}
+
+ColMatrix::ColMatrix(int m){  // size constructor
+	numrows = 0;
+	elements = 0;
+	Dim(m);
+}
+
+void ColMatrix::Dim(int m){
+	delete [] elements;
+	numrows = m;
+	elements = new double [m];
+}
+
+void ColMatrix::Const(double value){
+	for(int i=0;i<numrows;i++)
+		elements[i] = value;
+}
+
+MatrixType ColMatrix::GetType() const{
+	return COLMATRIX;
+}
+
+istream& ColMatrix::ReadData(istream& c){
+	int n;
+	c >> n;
+	Dim(n);
+	for(int i=0;i<n;i++)
+		c >> elements[i];
+	return c;
+}
+
+ostream& ColMatrix::WriteData(ostream& c) const{  //output
+	c << numrows << ' ';
+	for(int i=0;i<numrows;i++)
+		c << elements[i] << ' ';
+	return c;
+}
+
+double& ColMatrix::operator_1int (int i){ // array access
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+	return elements[i-1];
+}
+
+double ColMatrix::Get_1int(int i) const{
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index exceeded in Get" << endl;
+		exit(1);
+	}
+	return elements[i-1];
+}
+
+void ColMatrix::Set_1int(int i, double value){
+	if((i>numrows) || (i<1)){
+		cerr << "matrix index exceeded in Set" << endl;
+		exit(1);
+	}
+	elements[i-1] = value;
+}
+
+double ColMatrix::BasicGet_1int(int i) const{
+	return elements[i];
+}
+
+double* ColMatrix::GetElementPointer(int i){
+	return &(elements[i]);
+}
+
+void ColMatrix::BasicSet_1int(int i, double value){
+	elements[i] = value;
+}
+
+void ColMatrix::BasicIncrement_1int(int i, double value){
+	elements[i] += value;
+}
+
+void ColMatrix::AssignVM(const VirtualMatrix& A){
+	if( A.GetNumCols() != 1 ){
+		cerr << "error trying to write a 2D matrix to a collumn" << endl;
+		exit(1);
+	}
+	Dim( A.GetNumRows() );
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.BasicGet(i,0);
+}
+
+ColMatrix& ColMatrix::operator=(const ColMatrix& A){ // assignment operator
+	Dim(A.numrows);
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.elements[i];
+	return *this;
+}
+
+ColMatrix& ColMatrix::operator=(const VirtualColMatrix& A){ // overloaded =
+	Dim( A.GetNumRows() );
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.BasicGet(i);
+	return *this;
+}
+
+ColMatrix& ColMatrix::operator=(const VirtualMatrix& A){ // overloaded =
+	if( A.GetNumCols() != 1 ){
+		cerr << "error trying to write a 2D matrix to a collumn" << endl;
+		exit(1);
+	}
+	Dim( A.GetNumRows() );
+	for(int i=0;i<numrows;i++)
+		elements[i] = A.BasicGet(i,0);
+	return *this;
+}
+
+ColMatrix& ColMatrix::operator*=(double b){
+	for(int i=0;i<numrows;i++)
+		elements[i] *= b;
+	return *this;
+}
+
+void ColMatrix::Abs(){
+	for(int i=0;i<numrows;i++)
+		elements[i] = std::abs(elements[i]);
+}
+
+void ColMatrix::BasicMax(double& value, int& index){
+	value = elements[0];
+	index = 0;
+	for(int i=1;i<numrows;i++)
+		if(elements[i] > value){
+			value = elements[i];
+			index = i;
+		}
+}
+
+void ColMatrix::BasicMin(double& value, int& index){
+	value = elements[0];
+	index = 0;
+	for(int i=1;i<numrows;i++)
+		if(elements[i] < value){
+			value = elements[i];
+			index = i;
+		}
+}
+

lib/poems/colmatrix.h

@@ -0,0 +1,84 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: colmatrix.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef COLMATRIX_H
+#define COLMATRIX_H
+
+#include "virtualcolmatrix.h"
+#include "colmatmap.h"
+
+class Matrix;
+class Vect6;
+class Mat3x3;
+class Vect3;
+
+class ColMatrix : public VirtualColMatrix  {
+  double* elements;
+public:
+  ColMatrix();
+  ~ColMatrix();
+  ColMatrix(const ColMatrix& A);  // copy constructor
+  ColMatrix(const VirtualColMatrix& A);  // copy constructor
+  ColMatrix(const VirtualMatrix& A);  // copy constructor
+  ColMatrix(int m);  // size constructor
+
+  double& operator_1int (int i); // array access
+  double Get_1int(int i) const;
+  void Set_1int(int i, double value);
+  double BasicGet_1int(int i) const;
+  void BasicSet_1int(int i, double value);
+  void BasicIncrement_1int(int i, double value);
+
+  double* GetElementPointer(int i);
+  void Dim(int m);
+  void Const(double value);
+  MatrixType GetType() const;
+  std::istream& ReadData(std::istream& c);
+  std::ostream& WriteData(std::ostream& c) const;
+
+  void AssignVM(const VirtualMatrix& A);
+  ColMatrix& operator=(const ColMatrix& A); // assignment operator
+  ColMatrix& operator=(const VirtualColMatrix& A); // overloaded =
+  ColMatrix& operator=(const VirtualMatrix& A); // overloaded =
+  ColMatrix& operator*=(double b);
+
+        void Abs();
+        void BasicMax(double& value, int& index);
+        void BasicMin(double& value, int& index);
+
+  // fast matrix operations
+		  friend void FastQuaternions(ColMatrix& q, Mat3x3& C);
+		  friend void FastInvQuaternions(Mat3x3& C, ColMatrix& q);
+		  friend void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot);
+		  friend void FastTMult(Matrix& A, Vect6& B, ColMatrix& C);
+		  friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C);
+		  friend void FastAssign(ColMatrix& A, ColMatrix& C);
+
+		  friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
+		  friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);
+		  friend void FastAssign(ColMatrix&A, Vect3& C);
+  
+		  friend void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
+		  friend void EP_Transformation(ColMatrix& q, Mat3x3& C);
+		  friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C);
+		  friend void EP_Normalize(ColMatrix& q);
+		  friend void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot);
+		  friend void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
+};
+
+#endif

lib/poems/defines.h

@@ -0,0 +1,27 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: defines.h		                                          *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef _DEFINES_H_
+#define _DEFINES_H_
+
+enum SolverType {
+  ONSOLVER = 0,
+  PARTICLESOLVER = 1
+};
+
+#endif
+

lib/poems/eulerparameters.cpp

@@ -0,0 +1,163 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: eulerparameters.cpp                                     *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "eulerparameters.h"
+#include "math.h"
+
+using namespace std;
+
+void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){  
+  EP_Normalize(q);
+  int num=u.GetNumRows();
+  if (3<num){
+	  for (int i=4; i<=num; i++){
+		  qdot.elements[i]=u.elements[i-1];
+	  }
+  }
+  
+  qdot.elements[0] = 0.5 *(q.elements[3]*u.elements[0] - q.elements[2]*u.elements[1] + q.elements[1]*u.elements[2]);  
+  qdot.elements[1] = 0.5 *(q.elements[2]*u.elements[0] + q.elements[3]*u.elements[1] - q.elements[0]*u.elements[2]);
+  qdot.elements[2] = 0.5 *(-q.elements[1]*u.elements[0] + q.elements[0]*u.elements[1] + q.elements[3]*u.elements[2]);
+  qdot.elements[3] = -0.5 *(q.elements[0]*u.elements[0] + q.elements[1]*u.elements[1] + q.elements[2]*u.elements[2]);
+    
+  }
+
+void EP_Transformation(ColMatrix& q, Mat3x3& C){
+  EP_Normalize(q);
+  
+  double q11 = q.elements[0]*q.elements[0];
+  double q22 = q.elements[1]*q.elements[1];
+  double q33 = q.elements[2]*q.elements[2];
+  double q44 = q.elements[3]*q.elements[3];
+
+  double q12 = q.elements[0]*q.elements[1];
+  double q13 = q.elements[0]*q.elements[2];
+  double q14 = q.elements[0]*q.elements[3];
+  double q23 = q.elements[1]*q.elements[2];
+  double q24 = q.elements[1]*q.elements[3];
+  double q34 = q.elements[2]*q.elements[3];
+
+  C.elements[0][0] = q11 - q22 - q33 + q44;
+  C.elements[1][1] = -q11 + q22 - q33 + q44;
+  C.elements[2][2] = -q11 - q22 + q33 + q44;
+
+  C.elements[0][1] = 2*(q12 - q34);
+  C.elements[1][0] = 2*(q12 + q34);
+
+  C.elements[0][2] = 2*(q13 + q24);
+  C.elements[2][0] = 2*(q13 - q24);
+
+  C.elements[1][2] = 2*(q23 - q14);
+  C.elements[2][1] = 2*(q23 + q14);
+}
+
+void EP_FromTransformation(ColMatrix& q, Mat3x3& C){
+  double b[4];
+
+  // condition indicators
+  b[0] = C.elements[0][0] - C.elements[1][1] - C.elements[2][2] + 1;
+  b[1] = -C.elements[0][0] + C.elements[1][1] - C.elements[2][2] + 1;
+  b[2] = -C.elements[0][0] - C.elements[1][1] + C.elements[2][2] + 1;
+  b[3] = C.elements[0][0] + C.elements[1][1] + C.elements[2][2] + 1;
+
+  int max = 0;
+  for(int i=1;i<4;i++){
+    if( b[i] > b[max] ) max = i;
+  }
+
+  if( max == 3 ){
+    q.elements[3] = 0.5 * sqrt( b[3] );
+    q.elements[0] = ( C.elements[2][1] - C.elements[1][2] ) / ( 4 * q.elements[3] );
+    q.elements[1] = ( C.elements[0][2] - C.elements[2][0] ) / ( 4 * q.elements[3] );
+    q.elements[2] = ( C.elements[1][0] - C.elements[0][1] ) / ( 4 * q.elements[3] );
+    return;
+  }
+
+  if( max == 0 ){
+    q.elements[0] = 0.5 * sqrt( b[0] );
+    q.elements[1] = ( C.elements[0][1] + C.elements[1][0] ) / ( 4 * q.elements[0] );
+    q.elements[2] = ( C.elements[0][2] + C.elements[2][0] ) / ( 4 * q.elements[0] );
+    q.elements[3] = ( C.elements[2][1] - C.elements[1][2] ) / ( 4 * q.elements[0] );
+    return;
+  }
+
+  if( max == 1 ){
+    q.elements[1] = 0.5 * sqrt( b[1] );
+    q.elements[0] = ( C.elements[0][1] + C.elements[1][0] ) / ( 4 * q.elements[1] );
+    q.elements[2] = ( C.elements[1][2] + C.elements[2][1] ) / ( 4 * q.elements[1] );
+    q.elements[3] = ( C.elements[0][2] - C.elements[2][0] ) / ( 4 * q.elements[1] );
+    return;
+  }
+
+  if( max == 2 ){
+    q.elements[2] = 0.5 * sqrt( b[2] );
+    q.elements[0] = ( C.elements[0][2] + C.elements[2][0] ) / ( 4 * q.elements[2] );
+    q.elements[1] = ( C.elements[1][2] + C.elements[2][1] ) / ( 4 * q.elements[2] );
+    q.elements[3] = ( C.elements[1][0] - C.elements[0][1] ) / ( 4 * q.elements[2] );
+    return;
+  }
+  EP_Normalize(q);
+}
+
+void EP_Normalize(ColMatrix& q){
+  double one = 1.0/sqrt(q.elements[0]*q.elements[0] + q.elements[1]*q.elements[1] + q.elements[2]*q.elements[2] + q.elements[3]*q.elements[3]);  
+  q.elements[0] = one*q.elements[0];
+  q.elements[1] = one*q.elements[1];
+  q.elements[2] = one*q.elements[2];
+  q.elements[3] = one*q.elements[3];
+}
+
+
+
+
+void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq, ColMatrix& Aqddot){  
+  int num=Audot.GetNumRows();
+  if (3<num){
+	  for (int i=4; i<=num; i++){
+		  Aqddot.elements[i]=Audot.elements[i-1];
+	  }
+  }
+    
+  double AA;
+  AA=Aqdot.elements[0]*Aqdot.elements[0]+Aqdot.elements[1]*Aqdot.elements[1]+Aqdot.elements[2]*Aqdot.elements[2]+Aqdot.elements[3]*Aqdot.elements[3];
+  
+  Aqddot.elements[0] = 0.5 *(Aq.elements[3]*Audot.elements[0] - Aq.elements[2]*Audot.elements[1] + Aq.elements[1]*Audot.elements[2]-2*Aq.elements[0]*AA);
+  
+  Aqddot.elements[1] = 0.5 *(Aq.elements[2]*Audot.elements[0] + Aq.elements[3]*Audot.elements[1] - Aq.elements[0]*Audot.elements[2]-2*Aq.elements[1]*AA);
+  
+  Aqddot.elements[2] = 0.5 *(-Aq.elements[1]*Audot.elements[0] + Aq.elements[0]*Audot.elements[1] + Aq.elements[3]*Audot.elements[2]-2*Aq.elements[2]*AA);
+  
+  Aqddot.elements[3] = -0.5 *(Aq.elements[0]*Audot.elements[0] + Aq.elements[1]*Audot.elements[1] + Aq.elements[2]*Audot.elements[2]+2*Aq.elements[3]*AA);
+  
+}
+
+
+void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
+	EP_Normalize(q);
+	int num=qdot.GetNumRows();
+      	if (4<num){
+      		for (int i=5; i<=num; i++){
+      			u.elements[i-2]=qdot.elements[i-1];
+      		}
+      	}		
+      	u.elements[0]=2*(q.elements[3]*qdot.elements[0]+q.elements[2]*qdot.elements[1]-q.elements[1]*qdot.elements[2]-q.elements[0]*qdot.elements[3]);
+      	u.elements[1]=2*(-q.elements[2]*qdot.elements[0]+q.elements[3]*qdot.elements[1]+q.elements[0]*qdot.elements[2]-q.elements[1]*qdot.elements[3]);
+      	u.elements[2]=2*(q.elements[1]*qdot.elements[0]-q.elements[0]*qdot.elements[1]+q.elements[3]*qdot.elements[2]-q.elements[2]*qdot.elements[3]);
+  
+}
+
+

lib/poems/eulerparameters.h

@@ -0,0 +1,37 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: eulerparameters.h                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+#ifndef EULERPARAMETERS_H
+#define EULERPARAMETERS_H
+
+#include "colmatrix.h"
+#include "mat3x3.h"
+
+void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
+
+void EP_Transformation(ColMatrix& q, Mat3x3& C);
+
+void EP_FromTransformation(ColMatrix& q, Mat3x3& C);
+
+void EP_Normalize(ColMatrix& q);
+
+void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot);
+
+void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
+
+#endif
+

lib/poems/fastmatrixops.h

@@ -0,0 +1,97 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: fastmatrixops.h                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef FASTMATRIXOPS_H
+#define FASTMATRIXOPS_H
+#include "matrices.h"
+
+void FastCross(Vect3& a, Vect3& b, Vect3& c);
+void FastSimpleRotation(Vect3& v, double q, Mat3x3& C);
+void FastQuaternions(ColMatrix& q, Mat3x3& C);
+void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot);
+void FastLDLT(Matrix& A, Matrix& LD); // C is the LDL^T decomposition of A (SPD)
+void FastLDLT(Mat6x6& A, Mat6x6& LD); // C is the LDL^T decomposition of A (SPD)
+void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C); // Appropriate Forward and Back Substitution
+void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C); // Left handed Forward and Back Substitution
+void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C); // Appropriate Forward and Back Substitution
+void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C); // Appropriate Forward and Back Substitution
+void FastLU(Matrix& A, Matrix& LU, int *indx); // LU is the LU decomposition of A
+void FastLU(Mat3x3& A, Mat3x3& LU, int *indx); // LU is the LU decomposition of A
+void FastLU(Mat4x4& A, Mat4x4& LU, int *indx); // LU is the LU decomposition of A
+void FastLU(Mat6x6& A, Mat6x6& LU, int *indx); // LU is the LU decomposition of A
+void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx); // Appropriate Forward and Back Substitution
+void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx); // Appropriate Forward and Back Substitution
+void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx); // Appropriate Forward and Back Substitution
+void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx); // Appropriate Forward and Back Substitution
+// The following LUSubsLH routine is incomplete at the moment.
+void FastLUSubsLH(Matrix& B, Matrix& LU, Matrix& C, int *indx); // Appropriate Forward and Back Subsitution
+void FastTripleSum(Vect3& a, Vect3& b, Vect3& c, Vect3& d); // d = a+b+c
+void FastTripleSumPPM(Vect3& a, Vect3& b, Vect3& c, Vect3& d); // d = a+b-c
+
+void FastMult(Matrix& A, Matrix& B, Matrix& C);  // C = A*B
+void FastTMult(Matrix& A, Matrix& B, Matrix& C);  // C = A^T*B
+
+void FastMult(Mat3x3& A, Vect3& B, Vect3& C);  // C = A*B
+void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C); // C = A*B
+void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C); // C = A*B
+void FastMult(double a, Vect3& B, Vect3& C);  // C = a*B
+void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C);  // C = A*B
+void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C);  // C = A*B
+
+void FastMult(Mat4x4& A, Vect4& B, Vect4& C);  // C = A*B
+void FastTMult(Mat4x4& A, Vect4& B, Vect4& C);  // C = A^T*B
+void FastMult(double a, Vect4& B, Vect4& C);  // C = a*B
+void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C);  // C = A*B
+void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C);  // C = A*B
+
+void FastMultT(Matrix& A, Matrix& B, Mat6x6& C);  // C = A*B^T
+void FastMult(Mat6x6& A, Matrix& B, Matrix& C);   // C = A*B
+void FastTMult(Matrix& A, Vect6& B, ColMatrix& C);// C = A^T*B
+void FastMult(Matrix& A, ColMatrix& B, Vect6& C); // C = A*B
+
+void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C);  // C = A*B
+void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C);  // C = A*B
+
+void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C);  // C = A*B
+void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C);  // C = A*B
+
+void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A*B
+void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A*B^T
+void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A^T*B
+
+void FastMult(Mat6x6& A, Vect6& B, Vect6& C);  // C = A*B
+void FastTMult(Mat6x6& A, Vect6& B, Vect6& C);  // C = A^T*B
+
+void FastAdd(Vect3& A, Vect3& B, Vect3& C);     // C = A+B
+void FastAdd(Vect4& A, Vect4& B, Vect3& C);     // C = A+B
+void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A+B
+void FastAdd(Vect6& A, Vect6& B, Vect6& C);     // C = A+B
+
+void FastSubt(Vect3& A, Vect3& B, Vect3& C);    // C = A-B
+void FastSubt(Vect4& A, Vect4& B, Vect4& C);    // C = A-B
+void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C); // C = A-B
+void FastSubt(Vect6& A, Vect6& B, Vect6& C);    // C = A-B
+
+void FastAssign(ColMatMap& A, ColMatMap& C);  // C = A
+void FastAssign(ColMatrix& A, ColMatrix& C);  // C = A
+void FastAssign(Vect3& A, Vect3& C);          // C = A
+void FastAssign(ColMatrix&A, Vect3& C);
+void FastAssign(Vect4& A, Vect4& C);          // C = A
+void FastAssignT(Mat3x3& A, Mat3x3& C);       // C = A^T
+void FastAssignT(Mat4x4& A, Mat4x4& C);       // C = A^T
+
+#endif

lib/poems/fixedpoint.cpp

@@ -0,0 +1,55 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: fixedpoint.cpp                                          *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "fixedpoint.h"
+#include <iostream>
+#include <iomanip>
+
+using namespace std;
+
+FixedPoint::FixedPoint(){
+}
+FixedPoint::~FixedPoint(){
+}
+
+FixedPoint::FixedPoint(double x, double y, double z){
+  position(1) = x;
+  position(2) = y;
+  position(3) = z;
+}
+
+FixedPoint::FixedPoint(Vect3& v){
+  position = v;
+}
+
+PointType FixedPoint::GetType(){
+  return FIXEDPOINT;
+}
+
+bool FixedPoint::ReadInPointData(std::istream& in){
+  in >> position;
+  return true;
+}
+
+void FixedPoint::WriteOutPointData(std::ostream& out){  
+  out << setprecision(16) << position;
+}
+
+Vect3 FixedPoint::GetPoint(){
+  return position;
+}

lib/poems/fixedpoint.h

@@ -0,0 +1,37 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: fixedpoint.h                                            *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef FIXEDPOINT_H
+#define FIXEDPOINT_H
+
+#include "point.h"
+#include "vect3.h"
+
+class FixedPoint : public Point  {
+public:
+  FixedPoint();
+  ~FixedPoint();
+  FixedPoint(double x, double y, double z);
+  FixedPoint(Vect3& v);
+  PointType GetType(); 
+  Vect3 GetPoint();
+  bool ReadInPointData(std::istream& in);
+  void WriteOutPointData(std::ostream& out);
+};
+
+#endif

lib/poems/freebodyjoint.cpp

@@ -0,0 +1,156 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: freebodyjoint.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+#include "freebodyjoint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+#include "norm.h"
+#include "eulerparameters.h"
+#include "matrices.h"
+#include <iomanip>
+   
+
+FreeBodyJoint::FreeBodyJoint(){
+  DimQandU(7,6);  
+}
+
+FreeBodyJoint::~FreeBodyJoint(){
+}
+
+JointType FreeBodyJoint::GetType(){
+  return FREEBODYJOINT;
+}
+
+bool FreeBodyJoint::ReadInJointData(std::istream& in){
+  return true;
+}
+
+void FreeBodyJoint::WriteOutJointData(std::ostream& out){
+}
+
+void FreeBodyJoint::ComputeLocalTransform(){
+  Mat3x3 ko_C_k;  
+  EP_Transformation(q, ko_C_k);   
+  FastMult(pk_C_ko,ko_C_k,pk_C_k);  
+}
+
+Matrix FreeBodyJoint::GetForward_sP(){
+  Mat6x6 sP;
+  //sP.Identity();      
+  
+  sP.Zeros();  
+  Mat3x3 temp0=T(pk_C_k);
+  for(int i=1;i<4;i++){
+	  sP(i,i)=1.0;
+	  for(int j=1;j<4;j++){
+		  sP(3+i,3+j)=temp0(i,j);
+		  }
+	  } 	  	  
+  return sP;    
+}
+
+Matrix FreeBodyJoint::GetBackward_sP(){
+  Mat6x6 sP;
+  sP.Identity();        
+  sP =-1.0*sP;
+  cout<<"Did I come here in "<<endl;
+  return sP;    
+}
+
+
+void FreeBodyJoint::UpdateForward_sP( Matrix& sP){
+  // do nothing
+}
+  
+void FreeBodyJoint::UpdateBackward_sP( Matrix& sP){
+  // do nothing
+}  
+  
+void FreeBodyJoint::ForwardKinematics(){
+ //cout<<"Check in freebody "<<q<<" "<<qdot<<endl;
+  EP_Normalize(q);
+
+  // COMMENT STEP1: CALCULATE ORIENTATIONS
+  ComputeForwardTransforms();  
+  
+    
+  //COMMENT STEP2: CALCULATE POSITION VECTORS
+  Vect3 result1, result2, result3, result4; 
+  
+  result1.BasicSet(0,q.BasicGet(4));
+  result1.BasicSet(1,q.BasicGet(5));
+  result1.BasicSet(2,q.BasicGet(6));
+  
+  FastAssign(result1,r12);    
+  FastNegMult(k_C_pk,r12,r21);
+  
+  FastAssign(r12,body2->r);  
+    
+  //COMMENT STEP3: CALCULATE QDOT  
+  qdot_to_u(q, u, qdot);
+  
+  
+  Vect3 WN;
+  WN.BasicSet(0,u.BasicGet(0));
+  WN.BasicSet(1,u.BasicGet(1));
+  WN.BasicSet(2,u.BasicGet(2));
+    
+  Vect3 VN; 
+  VN.BasicSet(0,u.BasicGet(3));
+  VN.BasicSet(1,u.BasicGet(4));
+  VN.BasicSet(2,u.BasicGet(5));  
+    
+  FastAssign(WN,body2->omega_k);  
+ 
+  Vect3 pk_w_k;
+  FastMult(body2->n_C_k,WN,pk_w_k);      
+  FastAssign(pk_w_k,body2->omega);
+  
+
+  
+  //COMMENT STEP5: CALCULATE VELOCITES        
+  FastAssign(VN,body2->v);    
+  FastTMult(body2->n_C_k,body2->v,body2->v_k);        
+  
+  
+  //CALCULATE KE          
+ 
+  Matrix tempke;
+  tempke = T(body2->v)*(body2->v);    
+  double ke = 0.0;
+  ke = body2->mass*tempke(1,1);  
+  FastMult(body2->inertia,body2->omega_k,result1);
+  tempke= T(body2->omega_k)*result1;  
+  ke = 0.5*ke + 0.5*tempke(1,1);
+  body2->KE=ke;
+ 
+  
+  //COMMENT STEP6: CALCULATE STATE EXPLICIT ANGULAR ACCELERATIONS
+  body2->alpha_t.Zeros();
+    
+    
+  //COMMENT STEP7: CALCULATE STATE EXPLICIT ACCELERATIONS
+  body2->a_t.Zeros();   
+  
+  }
+
+void FreeBodyJoint::BackwardKinematics(){
+cout<<"Did I come here "<<endl;
+         
+}

lib/poems/freebodyjoint.h

@@ -0,0 +1,41 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: freebodyjoint.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef FREEBODYJOINT_H
+#define FREEBODYJOINT_H
+
+#include "joint.h"
+
+
+class FreeBodyJoint : public Joint{
+public: 
+  FreeBodyJoint();
+  ~FreeBodyJoint();
+  
+  JointType GetType();
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  void ComputeLocalTransform();
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/inertialframe.cpp

@@ -0,0 +1,58 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: inertialframe.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "inertialframe.h"
+#include "fixedpoint.h"
+
+using namespace std;
+
+InertialFrame::InertialFrame(){
+  gravity.Zeros();
+  n_C_k.Identity();
+
+  r.Zeros();
+  v.Zeros();
+  v_k.Zeros();
+  a.Zeros();
+  omega.Zeros();
+  omega_k.Zeros();
+  alpha.Zeros();
+  alpha_t.Zeros();
+}
+InertialFrame::~InertialFrame(){
+}
+
+BodyType InertialFrame::GetType(){
+  return INERTIALFRAME;
+}
+
+Vect3 InertialFrame::GetGravity(){
+  return gravity;
+}
+
+void InertialFrame::SetGravity(Vect3& g){
+  gravity = g;
+}
+
+bool InertialFrame::ReadInBodyData(istream& in){
+  in >> gravity;
+  return true;
+}
+
+void InertialFrame::WriteOutBodyData(ostream& out){
+  out << gravity;
+}

lib/poems/inertialframe.h

@@ -0,0 +1,37 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: inertialframe.h                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef INERTIALFRAME_H
+#define INERTIALFRAME_H
+
+#include "body.h"
+
+
+class InertialFrame : public Body  {
+  Vect3 gravity;
+public:
+  InertialFrame();
+  ~InertialFrame();
+  BodyType GetType();
+  Vect3 GetGravity();
+  void SetGravity(Vect3& g);
+  bool ReadInBodyData(std::istream& in);
+  void WriteOutBodyData(std::ostream& out);
+};
+
+#endif

lib/poems/joint.cpp

@@ -0,0 +1,248 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: joint.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "joints.h"
+#include "body.h"
+#include "point.h"
+#include <string>
+#include "matrixfun.h"
+#include "fastmatrixops.h"
+#include <iomanip>
+
+using namespace std;
+
+Joint::Joint(){
+  body1 = body2 = 0;
+  point1 = point2 = 0;
+  pk_C_ko.Identity();
+  pk_C_k.Identity();
+}
+
+Joint::~Joint(){
+}
+
+void Joint::SetBodies(Body* b1, Body* b2){
+  body1 = b1;
+  body2 = b2;
+}
+
+void Joint::SetPoints(Point* p1, Point* p2){
+  point1 = p1;
+  point2 = p2;
+}
+
+int Joint::GetBodyID1(){
+  return body1->GetID();
+}
+
+int Joint::GetBodyID2(){
+  return body2->GetID();
+}
+
+int Joint::GetPointID1(){
+  return point1->GetID();
+}
+
+int Joint::GetPointID2(){
+  return point2->GetID();
+}
+
+bool Joint::ReadIn(std::istream& in){
+  in >>setprecision(20)>> qo >> setprecision(20)>>qdoto >> setprecision(20)>>pk_C_ko; 
+  q = qo;
+  qdot = qdoto;
+  EP_Normalize(q);
+  
+  return ReadInJointData(in);
+}
+
+void Joint::ResetQdot(){
+  //EP_Derivatives(q,u,qdot);  
+  qdot_to_u(q,u,qdot);
+}
+
+void Joint::ResetQ(){
+  EP_Normalize(q); 
+}
+
+void Joint::SetInitialState(ColMatrix& a, ColMatrix& adot){
+  if( (qo.GetNumRows() != a.GetNumRows()) || (qdoto.GetNumRows() != adot.GetNumRows()) ){
+	  cout<<qo.GetNumRows()<<"	"<<a.GetNumRows()<<" "<<qdoto.GetNumRows()<<" "<<adot.GetNumRows()<<endl;	  
+    cerr << "ERROR::Illegal matrix size for initial condition" << endl;
+    exit(1);
+  }
+  qo = a;
+  qdoto = adot;
+  EP_Normalize(qo);
+  q=qo;           //////////////////////////Check this ...added May 14 2005
+  qdot=qdoto;     //////////////////////////Check this ...added May 14 2005  
+}
+
+void Joint::SetZeroOrientation(VirtualMatrix& C){
+  pk_C_ko = C;  
+}
+
+
+void Joint::WriteOut(std::ostream& out){  
+  out << GetType() << ' ' << GetName() << ' ';
+  out << GetBodyID1() << ' ' << GetBodyID2() << ' ';
+  out << GetPointID1() << ' ' << GetPointID2() << endl;
+  out <<setprecision(16)<< qo <<setprecision(16)<< qdoto <<setprecision(16)<< pk_C_ko;
+  WriteOutJointData(out);
+  out << endl;
+}
+
+ColMatrix* Joint::GetQ(){
+  return &q;
+}
+
+ColMatrix* Joint::GetU(){  
+  return &u;
+}
+
+ColMatrix* Joint::GetQdot(){  
+  return &qdot;
+}
+
+ColMatrix* Joint::GetUdot(){    
+  return &udot;
+}
+
+ColMatrix* Joint::GetQdotdot(){  
+  return &qdotdot;
+}
+
+
+void Joint::DimQandU(int i){
+  DimQandU(i,i);  
+}
+
+void Joint::DimQandU(int i, int j){
+  qo.Dim(i);
+  q.Dim(i);
+  qdot.Dim(i);
+  qdoto.Dim(i);
+  uo.Dim(j);
+  u.Dim(j);
+  udot.Dim(j);
+  qdotdot.Dim(i);  
+  
+  // zero them
+  qo.Zeros();
+  qdoto.Zeros();
+  q.Zeros();
+  qdot.Zeros();
+  uo.Zeros();
+  u.Zeros();  
+  udot.Zeros(); 
+  qdotdot.Zeros();
+  
+}
+
+Body* Joint::GetBody1(){
+  return body1;
+}
+
+Body* Joint::GetBody2(){
+  return body2;
+}
+
+Body* Joint::OtherBody(Body* body){
+  if(body1 == body) return body2;
+  if(body2 == body) return body1;
+  return 0;
+}
+
+Vect3* Joint::GetR12(){
+  return &r12; 
+}
+
+Vect3* Joint::GetR21(){
+  return &r21;
+}
+
+Mat3x3* Joint::Get_pkCk(){
+  return &pk_C_k;
+}
+
+
+Mat3x3* Joint::Get_kCpk(){
+  return &k_C_pk;
+}
+
+Matrix Joint::GetForward_sP(){
+  cerr << "ERROR: Forward Spatial Partial Velocity is not suported for joint type " << GetType() << endl;
+  exit(0);
+}
+
+Matrix Joint::GetBackward_sP(){
+  cerr << "ERROR: Backward Spatial Partial Velocity is not suported for joint type " << GetType() << endl;
+  exit(0);
+}
+
+void Joint::UpdateForward_sP(Matrix& sP){
+  cerr << "WARNING: Using default Update sP procedure" << endl;
+  sP = GetForward_sP();
+}
+
+void Joint::UpdateBackward_sP(Matrix& sP){
+  cerr << "WARNING: Using default Update sP procedure" << endl;
+  sP = GetBackward_sP();
+}
+
+void Joint::ComputeForwardTransforms(){
+  ComputeLocalTransform();
+//  k_C_pk = T(pk_C_k);
+  FastAssignT(pk_C_k, k_C_pk);
+  ComputeForwardGlobalTransform();  
+}
+
+void Joint::ComputeBackwardTransforms(){
+  ComputeLocalTransform();
+  // k_C_pk = pk_C_k^T
+  FastAssignT(pk_C_k, k_C_pk);
+  ComputeBackwardGlobalTransform();
+}
+
+void Joint::ComputeForwardGlobalTransform(){
+  // body2->n_C_k = body1->n_C_k * pk_C_k;
+  FastMult(body1->n_C_k,pk_C_k,body2->n_C_k);   
+  }
+
+void Joint::ComputeBackwardGlobalTransform(){
+  // body1->n_C_k = body2->n_C_k * T(pk_C_k);
+  FastMultT(body2->n_C_k,pk_C_k,body1->n_C_k);
+}
+
+
+//
+// global joint functions
+//
+
+Joint* NewJoint(int type){
+  switch( JointType(type) )
+  {
+    case FREEBODYJOINT : return new FreeBodyJoint;
+    case REVOLUTEJOINT : return new RevoluteJoint;
+    case PRISMATICJOINT : return new PrismaticJoint;
+    case SPHERICALJOINT : return new SphericalJoint;
+	 case BODY23JOINT : return new Body23Joint;
+	 case MIXEDJOINT : return new MixedJoint;
+    default : return 0; // error
+  }
+}

lib/poems/joint.h

@@ -0,0 +1,123 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: joint.h                                                 *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef JOINT_H
+#define JOINT_H
+
+#include "poemsobject.h"
+#include <iostream>
+#include "matrices.h"
+
+enum JointType {
+  XYZJOINT = 0,
+  FREEBODYJOINT = 1,
+  REVOLUTEJOINT = 2,
+  PRISMATICJOINT = 3,
+  SPHERICALJOINT = 4,
+  BODY23JOINT = 5,
+  MIXEDJOINT = 6		  
+};
+
+class Body;
+class Point;
+
+class Joint : public POEMSObject {
+protected:
+  Body* body1;
+  Body* body2;
+  Point* point1;
+  Point* point2;
+
+  ColMatrix qo;     // generalized coordinates (initial value)
+  ColMatrix uo;     // generalized speeds (initial value)
+  ColMatrix qdoto;     // generalized speeds (initial value)
+
+  ColMatrix q;      // generalized coordinates
+  ColMatrix u;      // generalized speeds
+  ColMatrix qdot;   // generalized coordinate derivatives
+  ColMatrix udot;   // generalized speed derivatives
+  ColMatrix qdotdot;
+  
+  Mat3x3 pk_C_ko;   // transformation relationship for q = 0
+
+  Mat3x3 pk_C_k;  // local transform
+  Mat3x3 k_C_pk;
+
+  Vect3 r12;
+  Vect3 r21;
+
+public:
+
+  Joint();
+  virtual ~Joint();
+  virtual JointType GetType() = 0;
+
+  void SetBodies(Body* b1, Body* b2);
+  void SetPoints(Point* p1, Point* p2);
+
+  int GetBodyID1();
+  int GetBodyID2();
+  int GetPointID1();
+  int GetPointID2();
+
+  ColMatrix* GetQ();
+  ColMatrix* GetU();
+  ColMatrix* GetQdot();
+  ColMatrix* GetUdot();
+  ColMatrix* GetQdotdot();
+  /*ColMatrix* GetAcc();
+  ColMatrix* GetAng();*/
+
+  void DimQandU(int i);
+  void DimQandU(int i, int j);
+
+  Body* GetBody1();
+  Body* GetBody2();
+  Body* OtherBody(Body* body);
+
+  Vect3* GetR12();
+  Vect3* GetR21();
+  Mat3x3* Get_pkCk();
+  Mat3x3* Get_kCpk();
+
+  //void SetInitialState(VirtualMatrix& q, VirtualMatrix& u);  
+  void SetInitialState(ColMatrix& q, ColMatrix& u);  
+  void SetZeroOrientation(VirtualMatrix& C);
+  void ResetQdot();
+  void ResetQ();
+  bool ReadIn(std::istream& in);  
+  void WriteOut(std::ostream& out);
+  
+  virtual void WriteOutJointData(std::ostream& out) = 0;
+  virtual bool ReadInJointData(std::istream& in) = 0;
+  virtual Matrix GetForward_sP();
+  virtual Matrix GetBackward_sP();
+  virtual void UpdateForward_sP(Matrix& sP);
+  virtual void UpdateBackward_sP(Matrix& sP);
+  virtual void ComputeForwardTransforms();
+  virtual void ComputeBackwardTransforms();
+  virtual void ComputeLocalTransform()=0;
+  virtual void ComputeForwardGlobalTransform();
+  virtual void ComputeBackwardGlobalTransform();
+  virtual void ForwardKinematics()=0;
+  virtual void BackwardKinematics()=0;
+};
+
+// global joint functions
+Joint* NewJoint(int type);
+
+#endif

lib/poems/joints.h

@@ -0,0 +1,30 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: joints.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef JOINTS_H
+#define JOINTS_H
+
+#include "joint.h"
+#include "freebodyjoint.h"
+#include "revolutejoint.h"
+#include "prismaticjoint.h"
+#include "sphericaljoint.h"
+#include "body23joint.h"
+#include "mixedjoint.h"
+
+#endif

lib/poems/mat3x3.cpp

@@ -0,0 +1,152 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat3x3.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "mat3x3.h"
+
+using namespace std;
+
+Mat3x3::Mat3x3(){
+  numrows = numcols = 3;
+}
+Mat3x3::~Mat3x3(){
+}
+
+Mat3x3::Mat3x3(const Mat3x3& A){
+  numrows = numcols = 3;
+  elements[0][0] = A.elements[0][0];elements[0][1] = A.elements[0][1];elements[0][2] = A.elements[0][2];
+  elements[1][0] = A.elements[1][0];elements[1][1] = A.elements[1][1];elements[1][2] = A.elements[1][2];
+  elements[2][0] = A.elements[2][0];elements[2][1] = A.elements[2][1];elements[2][2] = A.elements[2][2];
+}
+
+Mat3x3::Mat3x3(const VirtualMatrix& A){
+  numrows = numcols = 3;
+
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 3) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+  
+  for(int i=0;i<3;i++)
+    for(int j=0;j<3;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+double& Mat3x3::operator_2int(int i, int j){ // array access
+  return elements[i-1][j-1];
+}
+
+double Mat3x3::Get_2int(int i, int j) const{
+  return elements[i-1][j-1];
+}
+
+void Mat3x3::Set_2int(int i, int j, double value){
+  elements[i-1][j-1] = value;
+}
+
+double Mat3x3::BasicGet_2int(int i, int j) const{
+  return elements[i][j];
+}
+
+void Mat3x3::BasicSet_2int(int i, int j, double value){
+  elements[i][j] = value;
+}
+
+void Mat3x3::BasicIncrement_2int(int i, int j, double value){
+  elements[i][j] += value;
+}
+
+void Mat3x3::Const(double value){
+  elements[0][0] = elements[0][1] = elements[0][2] = value;
+  elements[1][0] = elements[1][1] = elements[1][2] = value;
+  elements[2][0] = elements[2][1] = elements[2][2] = value;
+}
+
+MatrixType Mat3x3::GetType() const{
+  return MAT3X3;
+}
+
+istream& Mat3x3::ReadData(istream& c){  //input
+  for(int i=0;i<3;i++)
+    for(int j=0;j<3;j++)
+      c >> elements[i][j];
+  return c;
+}
+
+ostream& Mat3x3::WriteData(ostream& c) const{  //output
+  for(int i=0;i<3;i++)
+    for(int j=0;j<3;j++)
+      c << elements[i][j] << ' ';
+  return c;
+}
+
+void Mat3x3::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 3) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+Mat3x3& Mat3x3::operator=(const Mat3x3& A){ // assignment operator
+  elements[0][0] = A.elements[0][0];elements[0][1] = A.elements[0][1];elements[0][2] = A.elements[0][2];
+  elements[1][0] = A.elements[1][0];elements[1][1] = A.elements[1][1];elements[1][2] = A.elements[1][2];
+  elements[2][0] = A.elements[2][0];elements[2][1] = A.elements[2][1];elements[2][2] = A.elements[2][2];
+  return *this;
+}
+
+Mat3x3& Mat3x3::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 3) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      elements[i][j] = A.BasicGet(i,j);
+  return *this;
+}
+
+Mat3x3& Mat3x3::operator*=(double b){
+  elements[0][0] *= b; elements[0][1] *= b; elements[0][2] *= b;
+  elements[1][0] *= b; elements[1][1] *= b; elements[1][2] *= b;
+  elements[2][0] *= b; elements[2][1] *= b; elements[2][2] *= b;
+  return *this;
+}
+
+Mat3x3& Mat3x3::Identity(){
+  elements[0][0] = 1.0;
+  elements[0][1] = 0.0;
+  elements[0][2] = 0.0;
+
+  elements[1][0] = 0.0;
+  elements[1][1] = 1.0;
+  elements[1][2] = 0.0;
+
+  elements[2][0] = 0.0;
+  elements[2][1] = 0.0;
+  elements[2][2] = 1.0;
+
+  return *this;
+}
+

lib/poems/mat3x3.h

@@ -0,0 +1,83 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat3x3.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef MAT3X3_H
+#define MAT3X3_H
+
+#include "virtualmatrix.h"
+
+
+class Vect3;
+class Mat6x6;
+class Matrix;
+class ColMatrix;
+
+class Mat3x3 : public VirtualMatrix  {
+  double elements[3][3];
+public:
+  Mat3x3();
+  ~Mat3x3();
+  Mat3x3(const Mat3x3& A);  // copy constructor
+  Mat3x3(const VirtualMatrix& A);  // copy constructor
+
+  double& operator_2int (int i, int j); // array access
+  double Get_2int(int i, int j) const;
+  void Set_2int(int i, int j, double value);
+  double BasicGet_2int(int i, int j) const;
+  void BasicSet_2int(int i, int j, double value);
+  void BasicIncrement_2int(int i, int j, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+  std::istream& ReadData(std::istream& c); // input
+  std::ostream& WriteData(std::ostream& c) const; // output
+
+  void AssignVM(const VirtualMatrix& A);
+  Mat3x3& operator=(const Mat3x3& A); // assignment operator
+  Mat3x3& operator=(const VirtualMatrix& A); // overloaded =
+  Mat3x3& operator*=(double b);
+
+  Mat3x3& Identity();
+
+  friend Mat3x3 T(const Mat3x3& A);  // a wasteful transpose
+  friend Mat3x3 CrossMat(Vect3& a);  // a wasteful cross matrix implementation
+
+  friend void FastSimpleRotation(Vect3& v, double q, Mat3x3& d);
+  friend void FastQuaternions(ColMatrix& q, Mat3x3& C);
+  friend void FastInvQuaternions(Mat3x3& C, ColMatrix& q);
+  friend void FastLU(Mat3x3& A, Mat3x3& LU, int *indx);
+  friend void FastLUSubs(Mat3x3& LU, Mat3x3& B, Mat3x3& C, int *indx);
+  friend void FastMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastTMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C);
+  friend void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C);
+  friend void FastAssignT(Mat3x3& A, Mat3x3& C);
+  friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);  
+  
+  friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
+  friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
+
+  friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
+  
+  friend void EP_Transformation(ColMatrix& q, Mat3x3& C);
+  friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C);
+  
+};
+
+#endif

lib/poems/mat4x4.cpp

@@ -0,0 +1,163 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat4x4.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "mat4x4.h"
+
+using namespace std;
+
+Mat4x4::Mat4x4(){
+  numrows = numcols = 4;
+}
+Mat4x4::~Mat4x4(){
+}
+
+Mat4x4::Mat4x4(const Mat4x4& A){
+  numrows = numcols = 4;
+  elements[0][0] = A.elements[0][0];elements[0][1] = A.elements[0][1];elements[0][2] = A.elements[0][2]; elements[0][3] = A.elements[0][3];
+  elements[1][0] = A.elements[1][0];elements[1][1] = A.elements[1][1];elements[1][2] = A.elements[1][2]; elements[1][3] = A.elements[1][3];
+  elements[2][0] = A.elements[2][0];elements[2][1] = A.elements[2][1];elements[2][2] = A.elements[2][2]; elements[2][3] = A.elements[2][3];
+  elements[3][0] = A.elements[3][0];elements[3][1] = A.elements[3][1];elements[3][2] = A.elements[3][2]; elements[3][3] = A.elements[3][3];
+}
+
+Mat4x4::Mat4x4(const VirtualMatrix& A){
+  numrows = numcols = 4;
+
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 4) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+  
+  for(int i=0;i<4;i++)
+    for(int j=0;j<4;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+double& Mat4x4::operator_2int(int i, int j){ // array access
+  return elements[i-1][j-1];
+}
+
+double Mat4x4::Get_2int(int i, int j) const{
+  return elements[i-1][j-1];
+}
+
+void Mat4x4::Set_2int(int i, int j, double value){
+  elements[i-1][j-1] = value;
+}
+
+double Mat4x4::BasicGet_2int(int i, int j) const{
+  return elements[i][j];
+}
+
+void Mat4x4::BasicSet_2int(int i, int j, double value){
+  elements[i][j] = value;
+}
+
+void Mat4x4::BasicIncrement_2int(int i, int j, double value){
+  elements[i][j] += value;
+}
+
+void Mat4x4::Const(double value){
+  elements[0][0] = elements[0][1] = elements[0][2] = elements[0][3] = value;
+  elements[1][0] = elements[1][1] = elements[1][2] = elements[1][3] = value;
+  elements[2][0] = elements[2][1] = elements[2][2] = elements[2][3] = value;
+  elements[3][0] = elements[3][1] = elements[3][2] = elements[3][3] = value;
+}
+
+MatrixType Mat4x4::GetType() const{
+  return MAT4X4;
+}
+
+istream& Mat4x4::ReadData(istream& c){  //input
+  for(int i=0;i<4;i++)
+    for(int j=0;j<4;j++)
+      c >> elements[i][j];
+  return c;
+}
+
+ostream& Mat4x4::WriteData(ostream& c) const{  //output
+  for(int i=0;i<4;i++)
+    for(int j=0;j<4;j++)
+      c << elements[i][j] << ' ';
+  return c;
+}
+
+void Mat4x4::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 4) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+Mat4x4& Mat4x4::operator=(const Mat4x4& A){ // assignment operator
+  elements[0][0] = A.elements[0][0];elements[0][1] = A.elements[0][1];elements[0][2] = A.elements[0][2]; elements[0][3] = A.elements[0][3];
+  elements[1][0] = A.elements[1][0];elements[1][1] = A.elements[1][1];elements[1][2] = A.elements[1][2]; elements[1][3] = A.elements[1][3];
+  elements[2][0] = A.elements[2][0];elements[2][1] = A.elements[2][1];elements[2][2] = A.elements[2][2]; elements[2][3] = A.elements[2][3];
+  elements[3][0] = A.elements[3][0];elements[3][1] = A.elements[3][1];elements[3][2] = A.elements[3][2]; elements[3][3] = A.elements[3][3];
+  return *this;
+}
+
+Mat4x4& Mat4x4::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 4) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      elements[i][j] = A.BasicGet(i,j);
+  return *this;
+}
+
+Mat4x4& Mat4x4::operator*=(double b){
+  elements[0][0] *= b; elements[0][1] *= b; elements[0][2] *= b; elements[0][3] *= b;
+  elements[1][0] *= b; elements[1][1] *= b; elements[1][2] *= b; elements[1][3] *= b;
+  elements[2][0] *= b; elements[2][1] *= b; elements[2][2] *= b; elements[2][3] *= b;
+  elements[3][0] *= b; elements[3][1] *= b; elements[3][2] *= b; elements[3][3] *= b;
+  return *this;
+}
+
+Mat4x4& Mat4x4::Identity(){
+  elements[0][0] = 1.0;
+  elements[0][1] = 0.0;
+  elements[0][2] = 0.0;
+  elements[0][3] = 0.0;
+
+  elements[1][0] = 0.0;
+  elements[1][1] = 1.0;
+  elements[1][2] = 0.0;
+  elements[1][3] = 0.0;
+
+  elements[2][0] = 0.0;
+  elements[2][1] = 0.0;
+  elements[2][2] = 1.0;
+  elements[2][3] = 0.0;
+
+  elements[3][0] = 0.0;
+  elements[3][1] = 0.0;
+  elements[3][2] = 0.0;
+  elements[3][3] = 1.0;
+
+  return *this;
+}
+

lib/poems/mat4x4.h

@@ -0,0 +1,68 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat4x4.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef MAT4X4_H
+#define MAT4X4_H
+
+#include "virtualmatrix.h"
+#include "matrix.h"
+
+
+class Vect4;
+
+class Mat4x4 : public VirtualMatrix  {
+  double elements[4][4];
+public: 
+  Mat4x4();
+  ~Mat4x4();
+  Mat4x4(const Mat4x4& A);  // copy constructor
+  Mat4x4(const VirtualMatrix& A);  // copy constructor
+  
+  double& operator_2int (int i, int j); // array access
+  double Get_2int(int i, int j) const;
+  void Set_2int(int i, int j, double value);
+  double BasicGet_2int(int i, int j) const;
+  void BasicSet_2int(int i, int j, double value);
+  void BasicIncrement_2int(int i, int j, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+  std::istream& ReadData(std::istream& c); // input
+  std::ostream& WriteData(std::ostream& c) const; // output
+
+  void AssignVM(const VirtualMatrix& A);
+  Mat4x4& operator=(const Mat4x4& A); // assignment operator
+  Mat4x4& operator=(const VirtualMatrix& A); // overloaded =
+  Mat4x4& operator*=(double b);
+
+  Mat4x4& Identity();
+
+  friend Mat4x4 T(const Mat4x4& A);  // a wasteful transpose
+  friend Mat4x4 CrossMat(Vect4& a);  // a wasteful cross matrix implementation
+
+  friend void FastLU(Mat4x4& A, Mat4x4& LU, int *indx);
+  friend void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx);
+  friend void FastMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastTMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C);
+  friend void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C);
+  friend void FastAssignT(Mat4x4& A, Mat4x4& C);
+};
+
+#endif

lib/poems/mat6x6.cpp

@@ -0,0 +1,152 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat6x6.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "mat6x6.h"
+
+using namespace std;
+
+Mat6x6::Mat6x6(){
+  numrows = numcols = 6;
+}
+Mat6x6::~Mat6x6(){
+}
+
+Mat6x6::Mat6x6(const Mat6x6& A){
+  numrows = numcols = 6;
+  int i,j;
+  for(i=0;i<6;i++)
+    for(j=0;j<6;j++)
+      elements[i][j] = A.elements[i][j];
+}
+
+Mat6x6::Mat6x6(const VirtualMatrix& A){
+  numrows = numcols = 6;
+
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 6) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<6;i++)
+    for(int j=0;j<6;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+double& Mat6x6::operator_2int(int i, int j){ // array access
+  return elements[i-1][j-1];
+}
+
+double Mat6x6::Get_2int(int i, int j) const{
+  return elements[i-1][j-1];
+}
+
+void Mat6x6::Set_2int(int i, int j, double value){
+  elements[i-1][j-1] = value;
+}
+
+double Mat6x6::BasicGet_2int(int i, int j) const{
+  return elements[i][j];
+}
+
+void Mat6x6::BasicSet_2int(int i, int j, double value){
+  elements[i][j] = value;
+}
+
+void Mat6x6::BasicIncrement_2int(int i, int j, double value){
+  elements[i][j] += value;
+}
+
+void Mat6x6::Const(double value){
+  int i,j;
+  for(i=0;i<6;i++)
+    for(j=0;j<6;j++)
+      elements[i][j] = value;
+}
+
+MatrixType Mat6x6::GetType() const{
+  return MAT6X6;
+}
+
+istream& Mat6x6::ReadData(istream& c){  //input
+  for(int i=0;i<6;i++)
+    for(int j=0;j<6;j++)
+      c >> elements[i][j];
+  return c;
+}
+
+ostream& Mat6x6::WriteData(ostream& c) const{  //output
+  for(int i=0;i<6;i++)
+    for(int j=0;j<6;j++)
+      c << elements[i][j] << ' ';
+  return c;
+}
+
+void Mat6x6::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 6) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<6;i++)
+    for(int j=0;j<6;j++)
+      elements[i][j] = A.BasicGet(i,j);
+}
+
+Mat6x6& Mat6x6::operator=(const Mat6x6& A){ // assignment operator
+  int i,j;
+  for(i=0;i<6;i++)
+    for(j=0;j<6;j++)
+      elements[i][j] = A.elements[i][j];
+  return *this;
+}
+
+Mat6x6& Mat6x6::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 6) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<6;i++)
+    for(int j=0;j<6;j++)
+      elements[i][j] = A.BasicGet(i,j);
+  return *this;
+}
+
+Mat6x6& Mat6x6::operator*=(double b){
+  int i,j;
+  for(i=0;i<6;i++)
+    for(j=0;j<6;j++)
+      elements[i][j] *= b;
+  return *this;
+}
+
+Mat6x6& Mat6x6::Identity(){
+  int i,j;
+  for(i=0;i<5;i++){
+    elements[i][i] = 1.0;
+    for(j=i+1;j<6;j++){
+      elements[i][j] = 0.0;
+      elements[j][i] = 0.0;
+    }
+  }
+  elements[i][i] = 1.0;
+  return *this;
+}
+

lib/poems/mat6x6.h

@@ -0,0 +1,76 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mat6x6.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef MAT6X6_H
+#define MAT6X6_H
+#include "virtualmatrix.h"
+
+
+class Matrix;
+class Mat3x3;
+class Vect6;
+class Vect3;
+
+class Mat6x6 : public VirtualMatrix  {
+  double elements[6][6];
+public:
+  Mat6x6();
+  ~Mat6x6();
+  Mat6x6(const Mat6x6& A);  // copy constructor
+  Mat6x6(const VirtualMatrix& A);  // copy constructor
+
+  double& operator_2int(int i, int j); // array access
+  double Get_2int(int i, int j) const;
+  void Set_2int(int i, int j, double value);
+  double BasicGet_2int(int i, int j) const;
+  void BasicSet_2int(int i, int j, double value);
+  void BasicIncrement_2int(int i, int j, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+
+  std::istream& ReadData(std::istream& c); // input
+  std::ostream& WriteData(std::ostream& c) const; // output
+
+  void AssignVM(const VirtualMatrix& A);
+  Mat6x6& operator=(const Mat6x6& A); // assignment operator
+  Mat6x6& operator=(const VirtualMatrix& A); // overloaded =
+  Mat6x6& operator*=(double b);
+
+  Mat6x6& Identity();
+
+  friend Mat6x6 T(const Mat6x6& A);  // a wasteful transpose
+
+  // fast matrix operations
+  friend void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C);
+  friend void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C);
+  friend void FastMultT(Matrix& A, Matrix& B, Mat6x6& C);
+  friend void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A*B
+  friend void FastMult(Mat6x6& A, Matrix& B, Matrix& C);
+  friend void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A*B^T
+  friend void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C);  // C = A^T*B
+  friend void FastMult(Mat6x6& A, Vect6& B, Vect6& C);
+  friend void FastTMult(Mat6x6& A, Vect6& B, Vect6& C);
+  friend void FastLDLT(Mat6x6& A, Mat6x6& C);
+  friend void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C);
+  friend void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C);
+
+  friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
+  friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
+};
+
+#endif

lib/poems/matrices.h

@@ -0,0 +1,42 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: matrices.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef MATRICES_H
+#define MATRICES_H
+
+#include "matrix.h"
+
+#include "colmatrix.h"
+
+#include "rowmatrix.h"
+
+#include "mat3x3.h"
+
+#include "vect3.h"
+
+#include "mat4x4.h"
+
+#include "vect4.h"
+
+#include "mat6x6.h"
+
+#include "vect6.h"
+
+#include "colmatmap.h"
+
+#endif

lib/poems/matrix.cpp

@@ -0,0 +1,166 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: matrix.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "matrix.h"
+#include <iostream>
+
+using namespace std;
+
+Matrix::Matrix(){
+  numrows = numcols = 0;
+  rows = 0;
+  elements = 0;
+}
+
+Matrix::~Matrix(){
+  delete [] rows;
+  delete [] elements;
+}
+
+Matrix::Matrix(const Matrix& A){
+  numrows = numcols = 0;
+  rows = 0;
+  elements = 0;
+  Dim(A.numrows,A.numcols);
+  for(int i=0;i<numrows*numcols;i++)
+    elements[i] = A.elements[i];
+}
+
+Matrix::Matrix(const VirtualMatrix& A){
+  numrows = numcols = 0;
+  rows = 0;
+  elements = 0;
+  Dim(A.GetNumRows(),A.GetNumCols());
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      rows[i][j] = A.BasicGet(i,j);
+}
+
+Matrix::Matrix(int m, int n){
+  numrows = numcols = 0;
+  rows = 0;
+  elements = 0;
+  this->Dim(m,n);
+}
+
+double& Matrix::operator_2int(int i, int j){
+  if((i>numrows) || (j>numcols) || (i*j==0)){
+    cerr << "matrix index exceeded in operator ()" << endl;
+    exit(1);
+  }
+  return rows[i-1][j-1];
+}
+
+double Matrix::Get_2int(int i, int j) const {
+  if((i>numrows) || (j>numcols) || (i*j==0)){
+    cerr << "matrix index exceeded in Get" << endl;
+    exit(1);
+  }
+  return rows[i-1][j-1];
+}
+
+void Matrix::Set_2int(int i, int j, double value){
+  if((i>numrows) || (j>numcols) || (i*j==0)){
+    cerr << "matrix index exceeded in Set" << endl;
+    exit(1);
+  }
+  rows[i-1][j-1] = value;
+}
+
+double Matrix::BasicGet_2int(int i, int j) const {
+  return rows[i][j];
+}
+
+void Matrix::BasicSet_2int(int i, int j, double value){
+  rows[i][j] = value;
+}
+
+void Matrix::BasicIncrement_2int(int i, int j, double value){
+  rows[i][j] += value;
+}
+
+void Matrix::Const(double value){
+  int num = numrows*numcols;
+  for(int i=0;i<num;i++) elements[i] = value;
+}
+
+MatrixType Matrix::GetType() const{
+  return MATRIX;
+}
+
+istream& Matrix::ReadData(istream& c){
+  int n,m;
+  c >> n >> m;
+  Dim(n,m);
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++){
+      c >> rows[i][j];
+    }
+  return c;
+}
+
+ostream& Matrix::WriteData(ostream& c) const{  //output
+  c << numrows << ' ' << numcols << ' ';
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++){
+      c << rows[i][j] << ' ';
+    }
+  return c;
+}
+
+Matrix& Matrix::Dim(int m, int n){  // allocate size
+  numrows = m;
+  numcols = n;
+  delete [] rows;
+  delete [] elements;
+  elements = new double[n*m];
+  rows = new double*[m];
+  for(int i=0;i<m;i++)
+    rows[i] = &elements[i*numcols];
+  return *this;
+}
+
+void Matrix::AssignVM(const VirtualMatrix& A){
+  Dim( A.GetNumRows(), A.GetNumCols() );
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      rows[i][j] = A.BasicGet(i,j);
+}
+
+Matrix& Matrix::operator=(const Matrix& A){
+  Dim(A.numrows,A.numcols);
+  for(int i=0;i<numrows*numcols;i++)
+    elements[i] = A.elements[i];
+  return *this;
+}
+
+Matrix& Matrix::operator=(const VirtualMatrix& A){
+  Dim( A.GetNumRows(), A.GetNumCols() );
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      rows[i][j] = A.BasicGet(i,j);
+  return *this;
+}
+
+Matrix& Matrix::operator*=(double b){
+  for(int i=0;i<numrows;i++)
+    for(int j=0;j<numcols;j++)
+      rows[i][j] *= b;
+  return *this;
+}
+

lib/poems/matrix.h

@@ -0,0 +1,77 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: matrix.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include "virtualmatrix.h"
+
+class Mat3x3;
+class Mat4x4;
+class Mat6x6;
+class Vect6;
+class ColMatrix;
+
+
+class Matrix : public VirtualMatrix {
+  double **rows;          // row pointer
+  double *elements;
+public:
+  Matrix();
+  ~Matrix();
+  Matrix(const Matrix& A);  // copy constructor
+  Matrix(const VirtualMatrix& A);  // copy constructor
+  Matrix(int m, int n);  // size constructor
+
+  double& operator_2int (int i, int j); // array access
+  double Get_2int(int i, int j) const;
+  void Set_2int(int i, int j, double value);
+  double BasicGet_2int(int i, int j) const;
+  void BasicSet_2int(int i, int j, double value);
+  void BasicIncrement_2int(int i, int j, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+  std::istream& ReadData(std::istream& c);
+  std::ostream& WriteData(std::ostream& c) const;
+
+  Matrix& Dim(int m, int n);    // allocate size
+
+  void AssignVM(const VirtualMatrix& A);
+  Matrix& operator=(const Matrix& A); // assignment operator
+  Matrix& operator=(const VirtualMatrix& A); // overloaded =
+  Matrix& operator*=(double b);
+
+  friend void FastLDLT(Matrix& A, Matrix& C);
+  friend void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C);
+  friend void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C);
+  friend void FastLU(Matrix& A, Matrix& LU, int *indx);
+  friend void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx);
+  friend void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx);
+  friend void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx);
+  friend void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx);
+  friend void FastLUSubsLH(Matrix& B, Matrix& LU, Matrix& C, int *indx);
+  friend void FastMult(Matrix& A, Matrix& B, Matrix& C);
+  friend void FastTMult(Matrix& A, Matrix& B, Matrix& C);
+  friend void FastTMult(Matrix& A, Vect6& B, ColMatrix& C);
+  friend void FastMult(Mat6x6& A, Matrix& B, Matrix& C);
+  friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C);
+  friend void FastMultT(Matrix& A, Matrix& B, Mat6x6& C);
+  
+};
+
+#endif

lib/poems/matrixfun.cpp

@@ -0,0 +1,407 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: matrixfun.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "matrixfun.h"
+#include <math.h>
+#include "fastmatrixops.h"
+
+using namespace std;
+
+//
+//  Create a new matrix
+//
+
+VirtualMatrix* NewMatrix(int type){
+  switch( MatrixType(type) )
+    {
+      case MATRIX : return new Matrix;
+      case COLMATRIX : return new ColMatrix;
+      case ROWMATRIX : return new RowMatrix;		
+      case MAT3X3 : return new Mat3x3;
+		case MAT4X4 : return new Mat4x4;
+      case VECT3 : return new Vect3;
+		case VECT4 : return new Vect4;
+      default  : return 0; // error!
+    }
+}
+
+//
+// Transpose
+//
+
+Matrix T(const VirtualMatrix& A){
+	int numrows = A.GetNumRows();
+	int numcols = A.GetNumCols();
+	Matrix C(numcols,numrows);
+	for(int i=0;i<numcols;i++)
+		for(int j=0;j<numrows;j++)
+			C.BasicSet(i,j,A.BasicGet(j,i));
+	return C;
+}
+
+Mat3x3 T(const Mat3x3& A){
+	Mat3x3 C;
+	C.elements[0][0] = A.elements[0][0];
+	C.elements[1][1] = A.elements[1][1];
+	C.elements[2][2] = A.elements[2][2];
+
+	C.elements[0][1] = A.elements[1][0];
+	C.elements[0][2] = A.elements[2][0];
+	C.elements[1][2] = A.elements[2][1];
+
+	C.elements[1][0] = A.elements[0][1];
+	C.elements[2][0] = A.elements[0][2];
+	C.elements[2][1] = A.elements[1][2];
+	return C;
+}
+
+Mat6x6 T(const Mat6x6& A){
+	Mat6x6 C;
+	int i,j;
+	for(i=0;i<6;i++)
+		for(j=0;j<6;j++)
+			C.elements[i][j] = A.elements[j][i];
+
+	return C;
+}
+
+Matrix T(const Vect3& A){
+	Matrix C(1,3);
+	C.BasicSet(0,0,A.elements[0]);
+	C.BasicSet(0,1,A.elements[1]);
+	C.BasicSet(0,2,A.elements[2]);
+
+	return C;
+}
+
+Matrix T(const Vect6& A){
+	Matrix C(1,6);
+	C.BasicSet(0,0,A.elements[0]);
+	C.BasicSet(0,1,A.elements[1]);
+	C.BasicSet(0,2,A.elements[2]);
+	C.BasicSet(0,3,A.elements[3]);
+	C.BasicSet(0,4,A.elements[4]);
+	C.BasicSet(0,5,A.elements[5]);
+
+	return C;
+}
+
+RowMatrix T(const VirtualColMatrix &A){
+	int numele = A.GetNumRows();
+	RowMatrix C(numele);
+	for(int i=0;i<numele;i++)
+		C.BasicSet(i,A.BasicGet(i));
+	return C;
+}
+
+ColMatrix T(const VirtualRowMatrix &A){
+	int numele = A.GetNumCols();
+	ColMatrix C(numele);
+	for(int i=0;i<numele;i++)
+		C.BasicSet(i,A.BasicGet(i));
+	return C;
+}
+
+//
+// Symmetric Inverse
+//
+
+Matrix SymInverse(Matrix &A){
+	int r = A.GetNumRows();
+	Matrix C(r,r);
+	Matrix LD(r,r);
+	Matrix I(r,r);
+	I.Zeros();
+	for(int i=0;i<r;i++) I.BasicSet(i,i,1.0);
+	FastLDLT(A,LD);
+	FastLDLTSubs(LD,I,C);
+	return C;
+}
+
+Mat6x6 SymInverse(Mat6x6 &A){
+	Mat6x6 C;
+	Mat6x6 LD;
+	Mat6x6 I;
+	I.Zeros();
+	for(int i=0;i<6;i++) I.BasicSet(i,i,1.0);
+	FastLDLT(A,LD);
+	FastLDLTSubs(LD,I,C);
+	return C;
+}
+
+//
+// Inverse
+//
+
+Matrix Inverse(Matrix& A){
+	int r = A.GetNumRows();
+	int indx[10000];
+	Matrix LU(r,r);
+	Matrix I(r,r);
+	Matrix C(r,r);
+	I.Zeros();
+	for(int i=0;i<r;i++) I.BasicSet(i,i,1.0);
+	FastLU(A,LU,indx);
+	FastLUSubs(LU,I,C,indx);
+	return C;
+}
+
+Mat3x3 Inverse(Mat3x3& A){
+	int indx[10000];
+	Mat3x3 LU;
+	Matrix I(3,3);
+	Matrix C(3,3);
+	I.Zeros();
+	for(int i=0;i<3;i++) I.BasicSet(i,i,1.0);
+	FastLU(A,LU,indx);
+	FastLUSubs(LU,I,C,indx);
+	return C;
+}
+
+Mat4x4 Inverse(Mat4x4& A){
+	int indx[10000];
+	Mat4x4 LU;
+	Matrix I(4,4);
+	Matrix C(4,4);
+	I.Zeros();
+	for(int i=0;i<4;i++) I.BasicSet(i,i,1.0);
+	FastLU(A,LU,indx);
+	FastLUSubs(LU,I,C,indx);
+	return C;
+}
+
+Mat6x6 Inverse(Mat6x6& A){
+	int indx[10000];
+	Mat6x6 LU;
+	Matrix I(6,6);
+	Matrix C(6,6);
+	I.Zeros();
+	for(int i=0;i<6;i++) I.BasicSet(i,i,1.0);
+	FastLU(A,LU,indx);
+	FastLUSubs(LU,I,C,indx);
+	return C;
+}
+
+//
+// overloaded addition
+//
+
+Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B){      // addition
+	int Arows,Acols,Brows,Bcols;
+	Arows = A.GetNumRows();
+	Acols = A.GetNumCols();
+	Brows = B.GetNumRows();
+	Bcols = B.GetNumCols();
+
+	if( !((Arows == Brows) && (Acols == Bcols)) ){
+		cerr << "Dimesion mismatch in matrix addition" << endl;
+		exit(1);
+	}
+
+	Matrix C(Arows,Acols);
+
+	for(int i=0;i<Arows;i++)
+		for(int j=0;j<Acols;j++)
+			C.BasicSet(i,j,A.BasicGet(i,j) + B.BasicGet(i,j));
+
+	return C;
+}
+
+//
+// overloaded subtraction
+//
+
+Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B){      // subtraction
+	int Arows,Acols,Brows,Bcols;
+	Arows = A.GetNumRows();
+	Acols = A.GetNumCols();
+	Brows = B.GetNumRows();
+	Bcols = B.GetNumCols();
+
+	if( !((Arows == Brows) && (Acols == Bcols)) ){
+		cerr << "Dimesion mismatch in matrix addition" << endl;
+		exit(1);
+	}
+
+	Matrix C(Arows,Acols);
+
+	for(int i=0;i<Arows;i++)
+		for(int j=0;j<Acols;j++)
+			C.BasicSet(i,j,A.BasicGet(i,j) - B.BasicGet(i,j));
+
+	return C;
+}
+
+//
+// overloaded matrix multiplication
+//
+
+Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B){      // multiplication
+	int Arows,Acols,Brows,Bcols;
+	Arows = A.GetNumRows();
+	Acols = A.GetNumCols();
+	Brows = B.GetNumRows();
+	Bcols = B.GetNumCols();
+
+	if(Acols != Brows){
+		cerr << "Dimesion mismatch in matrix multiplication" << endl;
+		exit(1);
+	}
+
+	Matrix C(Arows,Bcols);
+	C.Zeros();
+
+	for(int i=0;i<Arows;i++)
+		for(int j=0;j<Bcols;j++)
+			for(int k=0;k<Brows;k++)
+				C.BasicIncrement(i,j, A.BasicGet(i,k) * B.BasicGet(k,j) );
+
+	return C;
+}
+
+//
+// overloaded scalar multiplication
+//
+
+Matrix operator* (const VirtualMatrix &A, double b){    // multiplication
+	Matrix C = A;
+	C *= b;
+	return C;
+}
+
+Matrix operator* (double b, const VirtualMatrix &A){    // multiplication
+	Matrix C = A;  
+	C *= b;
+	return C;
+}
+
+//
+// overloaded negative
+//
+
+Matrix operator- (const VirtualMatrix& A){  // negative
+	int r = A.GetNumRows();
+	int c = A.GetNumCols();
+	Matrix C(r,c);
+
+	for(int i=0;i<r;i++)
+		for(int j=0;j<c;j++)
+			C.BasicSet(i,j,-A.BasicGet(i,j));
+
+	return C;
+}
+
+//
+// Cross product (friend of Vect3)
+//
+
+Vect3 Cross(Vect3& a, Vect3& b){
+	return CrossMat(a)*b;
+}
+
+//
+// Cross Matrix (friend of Vect3 & Mat3x3)
+//
+
+Mat3x3 CrossMat(Vect3& a){
+	Mat3x3 C;
+	C.Zeros();
+	C.elements[0][1] = -a.elements[2];
+	C.elements[1][0] = a.elements[2];
+	C.elements[0][2] = a.elements[1];
+	C.elements[2][0] = -a.elements[1];
+	C.elements[1][2] = -a.elements[0];
+	C.elements[2][1] = a.elements[0];
+
+	return C;
+}
+
+//
+// Stack
+//
+
+Matrix Stack(VirtualMatrix& A, VirtualMatrix& B){
+	int m,na,nb;
+	m = A.GetNumCols();
+	if( m != B.GetNumCols()){
+		cerr << "Error: cannot stack matrices of differing column dimension" << endl;
+		exit(0);
+	}
+	na = A.GetNumRows();
+	nb = B.GetNumRows();
+
+	Matrix C(na+nb,m);
+
+	for(int i=0;i<na;i++){
+		for(int j=0;j<m;j++){
+			C.BasicSet(i,j,A.BasicGet(i,j));
+		}
+	}
+
+	for(int i=0;i<nb;i++){
+		for(int j=0;j<m;j++){
+			C.BasicSet(i+na,j,B.BasicGet(i,j));
+		}
+	}
+
+	return C;
+}
+
+//Hstack
+
+Matrix HStack(VirtualMatrix& A, VirtualMatrix& B){
+	int m,na,nb;
+	m = A.GetNumRows();
+	if( m != B.GetNumRows()){
+		cerr << "Error: cannot stack matrices of differing row dimension" << endl;
+		exit(0);
+	}
+	na = A.GetNumCols();
+	nb = B.GetNumCols();
+
+	Matrix C(m,na+nb);
+
+	for(int i=0;i<m;i++){
+		for(int j=0;j<na;j++){
+			C.BasicSet(i,j,A.BasicGet(i,j));
+		}
+	}
+
+	for(int i=0;i<m;i++){
+		for(int j=0;j<nb;j++){
+			C.BasicSet(i,j+na,B.BasicGet(i,j));
+		}
+	}
+
+	return C;
+}
+
+//
+//
+
+void Set6DAngularVector(Vect6& v6, Vect3& v3){
+	v6.elements[0] = v3.elements[0];
+	v6.elements[1] = v3.elements[1];
+	v6.elements[2] = v3.elements[2];
+}
+
+void Set6DLinearVector(Vect6& v6, Vect3& v3){
+	v6.elements[3] = v3.elements[0];
+	v6.elements[4] = v3.elements[1];
+	v6.elements[5] = v3.elements[2];
+}
+

lib/poems/matrixfun.h

@@ -0,0 +1,76 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: matrixfun.h                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+#ifndef MATRIXFUN_H
+#define MATRIXFUN_H
+
+#include "matrices.h"
+
+// Create a Matrix
+VirtualMatrix* NewMatrix(int type);
+
+// Transpose
+Matrix T(const VirtualMatrix& A);
+Mat3x3 T(const Mat3x3& A);
+Mat6x6 T(const Mat6x6& A);
+Matrix T(const Vect3& A);
+Matrix T(const Vect6& A);
+RowMatrix T(const VirtualColMatrix& A);
+ColMatrix T(const VirtualRowMatrix& A);
+
+// Symmetric Inverse
+
+Matrix SymInverse(Matrix& A);
+Mat6x6 SymInverse(Mat6x6& A);
+
+// Inverse
+Matrix Inverse(Matrix& A);
+Mat3x3 Inverse(Mat3x3& A);
+Mat4x4 Inverse(Mat4x4& A);
+Mat6x6 Inverse(Mat6x6& A);
+
+// overloaded addition
+Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B);	// addition
+//Mat3x3 operator+ (const Mat3x3 &A, const Mat3x3 &B);	// addition
+//Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B);	// addition
+
+// overloaded subtraction
+Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B);	// subtraction
+
+// overloaded matrix multiplication
+Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B);  // multiplication
+
+// overloaded scalar-matrix multiplication
+Matrix operator* (const VirtualMatrix &A, double b);			// overloaded *
+Matrix operator* (double b, const VirtualMatrix &A);			// overloaded *
+
+// overloaded negative
+Matrix operator- (const VirtualMatrix &A);	// negative
+
+Vect3 Cross(Vect3& a, Vect3& b);
+Mat3x3 CrossMat(Vect3& a);
+
+Matrix Stack(VirtualMatrix& A, VirtualMatrix& B);
+Matrix HStack(VirtualMatrix& A, VirtualMatrix& B);
+
+void Set6DAngularVector(Vect6& v6, Vect3& v3);
+void Set6DLinearVector(Vect6& v6, Vect3& v3);
+
+
+
+
+#endif

lib/poems/mixedjoint.cpp

@@ -0,0 +1,196 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mixedjoint.cpp 	                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+#include "mixedjoint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+#include "norm.h"
+#include "eulerparameters.h"
+#include "matrices.h"
+
+   
+
+MixedJoint::MixedJoint(){		 
+}
+
+MixedJoint::~MixedJoint(){
+}
+
+JointType MixedJoint::GetType(){
+  return MIXEDJOINT;
+}
+
+bool MixedJoint::ReadInJointData(std::istream& in){
+  return true;
+}
+
+void MixedJoint::WriteOutJointData(std::ostream& out){
+}
+
+void MixedJoint::SetsP(Matrix& sPr, Vect6& temp_dofs, int i, int j){
+	const_sP = sPr;
+	dofs = temp_dofs;
+	numrots = i;
+	numtrans = j;
+	if (numrots < 2)
+		DimQandU(numrots+numtrans,numrots+numtrans);  
+	else 
+		DimQandU((4+numtrans),(numrots+numtrans)); 
+	cout<<"Check "<<4+numtrans<<" "<<numrots+numtrans<<" "<<i<<" "<<j<<endl;
+}
+
+void MixedJoint::ComputeLocalTransform(){
+  Mat3x3 ko_C_k;  
+  EP_Transformation(q, ko_C_k);   
+  FastMult(pk_C_ko,ko_C_k,pk_C_k);  
+}
+
+Matrix MixedJoint::GetForward_sP(){
+  Mat6x6 temp_sP;
+  Matrix sP;
+    
+  temp_sP.Zeros();  
+  Mat3x3 temp0=T(pk_C_k);
+  for(int i=1;i<4;i++){
+	  temp_sP(i,i)=1.0;
+	  for(int j=1;j<4;j++){
+		  temp_sP(3+i,3+j)=temp0(i,j);
+	  }
+  }
+  sP = temp_sP*const_sP;  
+  return sP;    
+}
+
+Matrix MixedJoint::GetBackward_sP(){
+  Mat6x6 sP;
+  sP.Identity();        
+  sP =-1.0*sP;
+  cout<<"Did I come here in "<<endl;
+  return sP;    
+}
+
+
+void MixedJoint::UpdateForward_sP( Matrix& sP){
+  // do nothing
+}
+  
+void MixedJoint::UpdateBackward_sP( Matrix& sP){
+  // do nothing
+}  
+  
+void MixedJoint::ForwardKinematics(){	
+	if(numrots > 1)
+		EP_Normalize(q);
+
+  // COMMENT STEP1: CALCULATE ORIENTATIONS
+  ComputeForwardTransforms();  
+  
+    
+  //COMMENT STEP2: CALCULATE POSITION VECTORS
+  Vect3 result1, result2, result3, result4;   
+  result1.Zeros();
+  for (int k=0; k<3; k++){
+	  if( dofs(3+k) != 0.0 ){
+		  if (numrots > 1)
+			  result1.BasicSet(k,q.BasicGet(4 + k));
+		  else
+			  result1.BasicSet(k,q.BasicGet(numrots + k));
+	  }  
+  }
+    
+  
+  
+  FastAssign(result1,r12);     // r12 in parents basis  i.e. Newtonian  
+  FastNegMult(k_C_pk,r12,r21);	    // r21 in body basis           
+  
+  FastAssign(r12,body2->r);  // This is right     
+    
+  //COMMENT STEP3: CALCULATE QDOT  
+  int pp = 0;
+  if (numrots > 1){
+	  ColMatrix temp_u(3+numtrans);
+	  qdot_to_u(q,temp_u,qdot);
+	  for (int k=1;k<=6;k++){
+		  if(dofs(k) != 0.0){
+			  u.BasicSet(pp,temp_u.BasicGet(k-1));			  
+			  pp = pp+1;
+		  }		  
+	  }
+  }
+  else u = qdot;
+
+  
+  
+  Vect3 WN; WN.Zeros();
+  int p = 0;
+  for (int k=0;k<3;k++){
+	  if(dofs(k+1) != 0.0){
+		  WN.BasicSet(k,u.BasicGet(p));
+		  p=p+1;
+	  }
+  }// WN is in body basis
+
+  
+  Vect3 VN; VN.Zeros();  
+  for (int k=0;k<3;k++){
+	  if( dofs(3+k+1) != 0.0 ) {
+		  VN.BasicSet(k,u.BasicGet(p));
+		  p=p+1;
+	  }
+  }// VN is the vector of translational velocities in Newtonian basis
+    
+  FastAssign(WN,body2->omega_k);  
+
+  // cout<<"Angular Velocity "<<WN<<endl;  
+  Vect3 pk_w_k;
+  FastMult(body2->n_C_k,WN,pk_w_k);      
+  FastAssign(pk_w_k,body2->omega);
+  
+  
+  
+  //COMMENT STEP5: CALCULATE VELOCITES        
+  FastAssign(VN,body2->v);    
+  FastTMult(body2->n_C_k,body2->v,body2->v_k);        
+  
+  
+  //CALCULATE KE          
+ 
+  Matrix tempke;
+  tempke = T(body2->v)*(body2->v);    
+  double ke = 0.0;
+  ke = body2->mass*tempke(1,1);    
+  FastMult(body2->inertia,body2->omega_k,result1);
+  tempke= T(body2->omega_k)*result1;  
+  ke = 0.5*ke + 0.5*tempke(1,1);
+  body2->KE=ke;
+  
+  
+  //COMMENT STEP6: CALCULATE STATE EXPLICIT ANGULAR ACCELERATIONS
+  body2->alpha_t.Zeros();
+      
+    
+  //COMMENT STEP7: CALCULATE STATE EXPLICIT ACCELERATIONS
+  body2->a_t.Zeros();   
+  
+  }
+
+void MixedJoint::BackwardKinematics(){
+cout<<"Did I come here "<<endl;
+         
+}

lib/poems/mixedjoint.h

@@ -0,0 +1,46 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: mixedjoint.h		                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef MIXEDJOINT_H
+#define MIXEDJOINT_H
+
+#include "joint.h"
+
+
+class MixedJoint : public Joint{
+  Matrix const_sP;
+  int numrots;
+  int numtrans;
+  Vect6 dofs;
+public: 
+  MixedJoint();
+  ~MixedJoint();
+  
+  JointType GetType();
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  void ComputeLocalTransform();
+  void SetsP(Matrix& sPr, Vect6& temp_dofs, int i, int j);
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/norm.cpp

@@ -0,0 +1,59 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: nrom.cpp                                               *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include <cmath>
+#include "norm.h"
+
+double Magnitude(ColMatrix& A){
+	double G;
+	G = 0;
+	for (int i=1;i<=A.GetNumRows();i++) G += A.Get(i)*A.Get(i);
+	G = sqrt(G);
+	return G;
+}
+
+double Magnitude(RowMatrix& A){
+	double G;
+	G = 0;
+	for (int i=1;i<=A.GetNumCols();i++) G += A.Get(i)*A.Get(i);
+	G = sqrt(G);
+	return G;
+}
+
+double Magnitude(Vect3& A){
+	double G;
+	G = 0;
+	for (int i=1;i<=3;i++) G += A.Get(i)*A.Get(i);
+	G = sqrt(G);
+	return G;
+}
+
+double Magnitude(Vect4& A){
+	double G;
+	G = 0;
+	for (int i=1;i<=4;i++) G += A.Get(i)*A.Get(i);
+	G = sqrt(G);
+	return G;
+}
+
+double Magnitude(Vect6& A){
+	double G;
+	G = 0;
+	for (int i=1;i<=6;i++) G += A.Get(i)*A.Get(i);
+	G = sqrt(G);
+	return G;
+}

lib/poems/norm.h

@@ -0,0 +1,30 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: norm.h                                                  *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef NORM_H
+#define NORM_H
+
+#include "matrices.h"
+
+
+double Magnitude(ColMatrix& A);
+double Magnitude(RowMatrix& A);
+double Magnitude(Vect3& A);
+double Magnitude(Vect4& A);
+double Magnitude(Vect6& A);
+
+#endif

lib/poems/onbody.cpp

@@ -0,0 +1,401 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onbody.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "onbody.h"
+#include "body.h"
+#include "inertialframe.h"
+#include "joint.h"
+#include "onfunctions.h"
+#include "virtualmatrix.h"
+#include "matrixfun.h"
+#include <iostream>
+#include "norm.h"
+#include "eulerparameters.h"
+
+using namespace std;
+
+
+OnBody::OnBody(){
+  system_body = 0;
+  system_joint = 0;
+  parent = 0;
+
+  // these terms have zeros which are NEVER overwritten
+  sI.Zeros();
+  sSC.Zeros();
+}
+
+OnBody::~OnBody(){
+  children.DeleteValues();
+}
+
+int OnBody::RecursiveSetup (InertialFrame* basebody){
+  int ID = 0;
+  system_body = basebody;
+
+  // record that the traversal algorithm has been here
+  if( system_body->GetID() ) return 0;
+  ID++;
+  system_body->SetID(ID);  
+
+  // setup inertial frame
+  SetupInertialFrame();
+
+  Joint* joint;
+  OnBody* child;
+  int tempID;
+
+  // loop through children calling the recursive setup function
+  ListElement<Joint>* ele = system_body->joints.GetHeadElement();
+  while(ele){
+    joint = ele->value;
+    child = new OnBody;
+
+    tempID = child->RecursiveSetup(ID,this,joint);
+    if( tempID ){
+      children.Append(child);
+      ID = tempID;      
+    }
+    else delete child;
+
+    ele = ele->next;
+  }
+
+  return ID;
+}
+
+int OnBody::RecursiveSetup(int ID, OnBody* parentbody, Joint* sys_joint){
+  // initialize the topology and system analogs
+  parent = parentbody;  
+  system_joint = sys_joint;
+  system_body = system_joint->OtherBody(parentbody->system_body);
+  
+    
+  // record that the traversal algorithm has been here
+  if( system_body->GetID() ) return 0;
+  ID++;  
+
+  // perform the local setup operations
+  Setup();
+
+  Joint* joint;
+  OnBody* child;
+  int tempID;
+
+  // loop through children calling the recursive setup function
+  ListElement<Joint>* ele = system_body->joints.GetHeadElement();
+  while(ele){
+    joint = ele->value;
+    if(joint != sys_joint){
+      child = new OnBody;
+
+      tempID = child->RecursiveSetup(ID,this,joint);
+      if( tempID ){
+        children.Append(child);
+        ID = tempID;
+      }
+      else delete child;
+    }
+
+    ele = ele->next;
+  }
+
+  return ID;  
+}
+
+void OnBody::SetupInertialFrame(){
+  // error check
+  if(system_body->GetType() != INERTIALFRAME){
+    cerr << "ERROR: attempting to setup inertial frame for non-inertial body" << endl;
+    exit(1);
+  }
+  
+  // setup gravity
+  Vect3 neg_gravity = -((InertialFrame*) system_body)->GetGravity();  
+  sAhat.Zeros();
+  Set6DLinearVector(sAhat,neg_gravity);
+  
+}
+
+
+void OnBody::Setup(){  
+  
+  // get the direction of the joint
+  if( system_joint->GetBody2() == system_body ) joint_dir = FORWARD;
+  else joint_dir = BACKWARD;
+
+  // set the function pointers for the joint direction
+  if( joint_dir == FORWARD ){    
+    kinfun = &Joint::ForwardKinematics;
+    updatesP = &Joint::UpdateForward_sP;
+    gamma = system_joint->GetR12(); 
+    pk_C_k = system_joint->Get_pkCk();
+  } else {
+    kinfun = &Joint::BackwardKinematics;
+    updatesP = &Joint::UpdateBackward_sP;
+    gamma = system_joint->GetR21();
+    pk_C_k = system_joint->Get_kCpk();
+  }
+
+  // initialize variables and dimensions
+
+  // sI
+  OnPopulateSI(system_body->inertia, system_body->mass, sI);
+	
+  // sP
+  if( joint_dir == FORWARD )
+    sP = system_joint->GetForward_sP();
+  else
+    sP = system_joint->GetBackward_sP();
+
+  // dimension these quantities
+  sM = T(sP)*sP;
+  sMinv = sM;
+  sPsMinv = sP;
+  sIhatsP = sP;
+  
+
+  // get the state and state derivative column matrix pointers
+  q = system_joint->GetQ();
+  u = system_joint->GetU();
+  qdot = system_joint->GetQdot();  
+  udot = system_joint->GetUdot();
+  qdotdot = system_joint->GetQdotdot();
+  }
+
+void OnBody::RecursiveKinematics(){
+  OnBody* child;
+  // Perform local kinematics recursively outward
+  ListElement<OnBody>* ele = children.GetHeadElement();
+  while(ele){
+    child = ele->value;    
+    child->LocalKinematics();
+    child->RecursiveKinematics();
+    Mat3x3 result=*child->pk_C_k;           
+    ele = ele->next;
+  }
+  
+}
+
+void OnBody::RecursiveTriangularization(){
+  OnBody* child;
+
+  // Perform local triangularization recursively inward
+  ListElement<OnBody>* ele = children.GetHeadElement();
+  while(ele){
+    child = ele->value;
+    child->RecursiveTriangularization();
+    //child->LocalTriangularization();
+    ele = ele->next;
+  }
+
+}
+
+void OnBody::RecursiveForwardSubstitution(){
+  OnBody* child;
+  // Perform local forward substitution recursively outward
+  ListElement<OnBody>* ele = children.GetHeadElement();
+  while(ele){
+    child = ele->value;
+   // child->LocalForwardSubstitution();
+    child->RecursiveForwardSubstitution();
+    ele = ele->next;
+  }
+}
+
+void OnBody::LocalKinematics(){
+  // do the directional kinematics
+  (system_joint->*kinfun)();
+  (system_joint->*updatesP)( sP );
+  OnPopulateSC( *gamma, *pk_C_k, sSC );      
+}
+
+Mat3x3 OnBody::GetN_C_K(){
+Mat3x3 nck=system_body->n_C_k;
+return nck;
+}
+
+ 
+int OnBody::GetBodyID(){
+int ID=system_body->GetID();
+return ID;
+}
+
+Vect3 OnBody::LocalCart(){  
+  (system_joint->*kinfun)();  
+  Vect3 RR=system_body->r;  
+  return RR;
+}
+
+  
+  
+void OnBody::LocalTriangularization(Vect3& Torque, Vect3& Force){	  
+
+	Vect3 Iw,wIw,Ialpha,wIwIalpha,ma,Bforce,Bforce_ma,Btorque,Btorque_wIwIalpha;
+  Iw.Zeros();wIw.Zeros();wIwIalpha.Zeros();ma.Zeros();
+	Bforce.Zeros();Bforce_ma.Zeros();Btorque.Zeros();Btorque_wIwIalpha.Zeros();
+     
+	FastMult(system_body->inertia,system_body->omega_k,Iw);  
+	FastCross(Iw,system_body->omega_k,wIw);  
+  
+	FastMult(system_body->inertia, system_body->alpha_t, Ialpha);    
+	FastSubt(wIw,Ialpha,wIwIalpha); 
+	FastMult(-system_body->mass,system_body->a_t,ma);    
+	
+	
+	Mat3x3 k_C_n=T(system_body->n_C_k);      
+	Bforce=k_C_n*Force;
+	Btorque=k_C_n*Torque;  
+	
+	FastAdd(Bforce,ma,Bforce_ma);       
+	FastAdd(Btorque,wIwIalpha,Btorque_wIwIalpha);	  
+	OnPopulateSVect(Btorque_wIwIalpha,Bforce_ma,sF);
+  
+  
+	sIhat = sI;    
+	sFhat = sF;     
+	Mat6x6 sPsMinvsPT;
+	Mat6x6 sIhatsPsMsPT;
+	Mat6x6 sUsIhatsPsMsPT;
+	Mat6x6 sTsIhat;
+	Mat6x6 sTsIhatsSCT;
+	Vect6 sTsFhat;
+	Mat6x6 sU;
+	sU.Identity();  
+  
+	OnBody* child;
+	ListElement<OnBody>* ele = children.GetHeadElement();
+  
+	while(ele){
+		child = ele->value;
+    
+		FastMultT(child->sIhatsP,child->sPsMinv,sIhatsPsMsPT); 
+		FastSubt(sU,sIhatsPsMsPT,sUsIhatsPsMsPT);              
+		FastMult(child->sSC,sUsIhatsPsMsPT,child->sT);         
+        
+		FastMult(child->sT,child->sIhat,sTsIhat);
+		FastMultT(sTsIhat,child->sSC,sTsIhatsSCT); 
+		FastAdd(sIhat,sTsIhatsSCT,sIhat);
+    
+		FastMult(child->sT,child->sFhat,sTsFhat);
+		FastAdd(sFhat,sTsFhat,sFhat);            
+		ele = ele->next;
+	}  
+  
+	FastMult(sIhat,sP,sIhatsP);   
+	FastTMult(sP,sIhatsP,sM);       
+	sMinv=SymInverse(sM);    
+	FastMult(sP,sMinv,sPsMinv);	
+}
+
+void OnBody::LocalForwardSubstitution(){  
+	Vect6 sSCTsAhat;
+	Vect6 sIhatsSCTsAhat;
+	Vect6 sFsIhatsSCTsAhat;
+	Vect6 sPudot;
+	int type = system_joint->GetType();
+	if(type == 1){
+		FastTMult(sSC,parent->sAhat,sSCTsAhat);
+		FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);       
+		FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
+		FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);		
+  
+		ColMatrix result1=*udot;      
+		ColMatrix result2=*qdot;
+		ColMatrix result3=*q;
+		int num=result1.GetNumRows();
+		ColMatrix result4(num+1);
+		result4.Zeros();		
+		EPdotdot_udot(result1, result2, result3, result4);
+		FastAssign(result4, *qdotdot);    
+		FastMult(sP,*udot,sPudot);      
+		FastAdd(sSCTsAhat,sPudot,sAhat);		
+	}
+	else if (type == 4){		
+		FastTMult(sSC,parent->sAhat,sSCTsAhat);
+		FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);       
+		FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
+		FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);
+		
+		ColMatrix result1=*udot;      		
+		ColMatrix result2=*qdot;
+		ColMatrix result3=*q;
+		int num=result1.GetNumRows();
+		ColMatrix result4(num+1);
+		result4.Zeros();
+		
+		EPdotdot_udot(result1, result2, result3, result4);
+		FastAssign(result4, *qdotdot);    
+		FastMult(sP,*udot,sPudot);      
+		FastAdd(sSCTsAhat,sPudot,sAhat);
+	}	
+	else if (type == 5){		
+		FastTMult(sSC,parent->sAhat,sSCTsAhat);
+		FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);       
+		FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
+		FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);				
+		ColMatrix temp_u = *udot;
+		
+		ColMatrix result1(3);		
+		result1(1)=0.0;
+		result1(2)=temp_u(1);      		
+		result1(3)=temp_u(2); 		
+		ColMatrix result2=*qdot;
+		ColMatrix result3=*q;
+		int num=result1.GetNumRows();
+		ColMatrix result4(num+1);
+		result4.Zeros();
+		
+		EPdotdot_udot(result1, result2, result3, result4);		
+		FastAssign(result4, *qdotdot);    
+		FastMult(sP,*udot,sPudot);      
+		FastAdd(sSCTsAhat,sPudot,sAhat);
+	}
+	else if (type == 6){		
+		FastTMult(sSC,parent->sAhat,sSCTsAhat);
+		FastMult(sIhat,sSCTsAhat,sIhatsSCTsAhat);       
+		FastSubt(sFhat,sIhatsSCTsAhat,sFsIhatsSCTsAhat);
+		FastTMult(sPsMinv,sFsIhatsSCTsAhat,*udot);				
+		ColMatrix temp_u = *udot;
+		int tt = temp_u.GetNumRows() + 1;
+		ColMatrix result1(tt);				
+		result1(1)=0.0;
+		for (int k =2; k<=tt; k++){
+			result1(k)=temp_u(k-1);      				
+		}			
+		ColMatrix result2=*qdot;
+		ColMatrix result3=*q;
+		int num=result1.GetNumRows();
+		ColMatrix result4(num+1);
+		result4.Zeros();
+		
+		EPdotdot_udot(result1, result2, result3, result4);		
+		FastAssign(result4, *qdotdot);    
+		FastMult(sP,*udot,sPudot);      
+		FastAdd(sSCTsAhat,sPudot,sAhat);
+	}		
+	else{
+		cout<<"Joint type not recognized in onbody.cpp LocalForwardSubsitution() "<<type<<endl;
+		exit(-1);
+	}
+	CalculateAcceleration();
+	
+}
+
+
+void OnBody::CalculateAcceleration(){
+}

lib/poems/onbody.h

@@ -0,0 +1,99 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onbody.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef ONBODY_H
+#define ONBODY_H
+
+#include "poemslist.h"
+#include "matrix.h"
+#include "vect6.h"
+#include "mat6x6.h"
+
+// emumerated type
+enum Direction {
+  BACKWARD = 0,
+  FORWARD= 1
+};
+
+class Body;
+class InertialFrame;
+class Joint;
+class OnSolver;
+
+class OnBody {
+       Body* system_body;
+       Joint* system_joint;
+       OnBody* parent;
+       List<OnBody> children;
+
+       Direction joint_dir;
+       void (Joint::*kinfun)();          // kinematics function
+       void (Joint::*updatesP)(Matrix&); // sP update function
+       Vect3* gamma;                     // pointer to gamma vector
+       Mat3x3* pk_C_k;                   // pointer to transformation
+
+  
+       Mat6x6 sI;      // spatial inertias
+       Mat6x6 sIhat;   // recursive spatial inertias
+       Mat6x6 sSC;     // spatial shift
+       Mat6x6 sT;      // spatial triangularization
+
+       Vect6 sF;       // spatial forces
+       Vect6 sFhat;    // recursive spatial forces
+       Vect6 sAhat;    // recursive spatial acceleration
+
+       Matrix sP;      // spatial partial velocities
+       Matrix sM;      // triangularized mass matrix diagonal elements
+       Matrix sMinv;   // inverse of sM
+       Matrix sPsMinv;
+       Matrix sIhatsP;
+
+       // states and state derivatives
+       ColMatrix* q;
+       ColMatrix* u;
+       ColMatrix* qdot;
+       ColMatrix* udot;
+       ColMatrix* qdotdot;
+	  
+       ColMatrix* r;       
+       ColMatrix* acc;
+       ColMatrix* ang;
+
+  // friend classes
+  friend class OnSolver;
+  
+
+public: 
+       OnBody();
+       ~OnBody();
+       int RecursiveSetup(InertialFrame* basebody);
+       int RecursiveSetup(int ID, OnBody* parentbody, Joint* sys_joint);
+       void RecursiveKinematics();
+       void RecursiveTriangularization();
+       void RecursiveForwardSubstitution();
+       Mat3x3 GetN_C_K();
+       Vect3 LocalCart();
+       int GetBodyID();
+       void CalculateAcceleration();       
+       void Setup();
+       void SetupInertialFrame();
+       void LocalKinematics();       
+		 void LocalTriangularization(Vect3& Torque, Vect3& Force);
+       void LocalForwardSubstitution();
+};
+
+#endif

lib/poems/onfunctions.cpp

@@ -0,0 +1,69 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onfunction.cpp                                          *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#include "onfunctions.h"
+#include "matrixfun.h"
+#include <iostream>
+using namespace std;
+
+// friend of Vect3 & Vect6
+void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV){
+  sV.elements[0] = angular.elements[0];
+  sV.elements[1] = angular.elements[1];
+  sV.elements[2] = angular.elements[2];
+  sV.elements[3] = linear.elements[0];
+  sV.elements[4] = linear.elements[1];
+  sV.elements[5] = linear.elements[2];
+}
+
+// friend of Vect3, Mat3x3, & Mat6x6
+void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC){
+  // the block diagonals
+  
+  // the gamma cross with transform
+  Mat3x3 temp; Mat3x3 temp2;
+  SC.Zeros();
+  temp.Zeros();  
+  temp2.Zeros();
+  //FastTMult(C,gamma,temp);     
+  temp(1,2)= -gamma(3); temp(1,3)= gamma(2); temp(2,1)= gamma(3);
+  temp(2,3)= -gamma(1); temp(3,1)= -gamma(2); temp(3,2)= gamma(1);  
+  FastMult(temp,C,temp2);
+  
+  SC(1,4)=temp2(1,1);   SC(2,4)=temp2(2,1);   SC(3,4)=temp2(3,1);
+  SC(1,5)=temp2(1,2);   SC(2,5)=temp2(2,2);   SC(3,5)=temp2(3,2);
+  SC(1,6)=temp2(1,3);   SC(2,6)=temp2(2,3);   SC(3,6)=temp2(3,3);
+  
+  SC(1,1)=C(1,1);   SC(2,1)=C(2,1);   SC(3,1)=C(3,1);
+  SC(1,2)=C(1,2);   SC(2,2)=C(2,2);   SC(3,2)=C(3,2);
+  SC(1,3)=C(1,3);   SC(2,3)=C(2,3);   SC(3,3)=C(3,3);
+
+  SC(4,4)=C(1,1);   SC(5,4)=C(2,1);   SC(6,4)=C(3,1);
+  SC(4,5)=C(1,2);   SC(5,5)=C(2,2);   SC(6,5)=C(3,2);
+  SC(4,6)=C(1,3);   SC(5,6)=C(2,3);   SC(6,6)=C(3,3);  
+  
+  }
+
+// friend of Mat3x3 & Mat6x6
+void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI){
+  
+	sI(4,4)=mass; sI(5,5)=mass; sI(6,6)=mass;
+	sI(1,1)=inertia(1,1); sI(1,2)=inertia(1,2); sI(1,3)=inertia(1,3);
+	sI(2,1)=inertia(2,1); sI(2,2)=inertia(2,2); sI(2,3)=inertia(2,3);
+	sI(3,1)=inertia(3,1); sI(3,2)=inertia(3,2); sI(3,3)=inertia(3,3);	
+}

lib/poems/onfunctions.h

@@ -0,0 +1,31 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onfunction.h                                            *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef ONFUNCTIONS_H
+#define ONFUNCTIONS_H
+
+#include "matrices.h"
+
+void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV);
+void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
+void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
+
+
+void Create_Map(int MM);
+int ICELL(int IX,int IY,int IZ, int MM);
+
+#endif

lib/poems/onsolver.cpp

@@ -0,0 +1,148 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onsolver.cpp                                            *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "onsolver.h"
+#include "system.h"
+#include "onbody.h"
+#include "body.h"
+#include "matrixfun.h"
+#include <fstream>
+
+
+using namespace std;
+
+OnSolver::OnSolver(){
+  numbodies = 0;
+  bodyarray = 0;
+  q=0;u=0; qdot=0; udot=0; qdotdot=0;  
+  type = ONSOLVER;
+}
+
+OnSolver::~OnSolver(){
+  DeleteModel();
+}
+
+void OnSolver::DeleteModel(){
+  delete [] bodyarray;
+
+  delete [] q;
+  delete [] u;
+
+  delete [] qdot;
+  delete [] udot; 
+  
+  delete [] qdotdot;
+  
+  numbodies = 0;
+}
+
+void OnSolver::CreateModel(){
+  // delete old model
+  DeleteModel();
+
+  // clear system body IDs (primer for traversal algorithm)
+  system->ClearBodyIDs();
+  
+
+  // error check for inertial frame
+  Body* sysbasebody = system->bodies.GetHeadElement()->value;
+  if( sysbasebody->GetType() != INERTIALFRAME ){
+    cerr << "ERROR: inertial frame not at head of bodies list" << endl;
+    exit(1);
+  }
+
+  // setup the O(n) spanning tree
+  numbodies = inertialframe.RecursiveSetup( (InertialFrame*) sysbasebody );
+  if(!numbodies){
+    cerr << "ERROR: unable to create O(n) model" << endl;
+    exit(1);
+  }
+  
+  
+  
+  bodyarray = new OnBody* [numbodies];
+  
+  CreateTopologyArray(0,&inertialframe);	  
+  
+  CreateStateMatrixMaps();  
+}
+
+int OnSolver::CreateTopologyArray(int num, OnBody* body){
+  int i = num;
+  bodyarray[i] = body;    
+  i++;
+
+  
+  OnBody* child;
+  ListElement<OnBody>* ele = body->children.GetHeadElement();    
+  
+  while(ele){
+    child = ele->value;    
+    i = CreateTopologyArray(i,child);
+    ele = ele->next;        
+  }  
+  return i;  
+}
+
+void OnSolver::CreateStateMatrixMaps(){
+  
+	
+  int numstates=0;
+  for(int i=1;i<numbodies;i++)    
+    numstates += bodyarray[i]->q->GetNumRows();       
+  
+  state.Dim(numstates);
+  statedot.Dim(numstates);
+  statedoubledot.Dim(numstates); 
+
+  
+  int count=0;
+  
+  for(int i=1;i<numbodies;i++){
+	  for(int j=0;j<bodyarray[i]->q->GetNumRows();j++){
+		  state.SetElementPointer(count,bodyarray[i]->q->GetElementPointer(j));      
+		  statedot.SetElementPointer(count,bodyarray[i]->qdot->GetElementPointer(j));        	
+		  statedoubledot.SetElementPointer(count,bodyarray[i]->qdotdot->GetElementPointer(j));       
+		  count++;      
+	  }        
+  }
+}
+
+
+void OnSolver::Solve(double time, Matrix& FF){
+	system->SetTime(time);
+	for(int i=1;i<numbodies;i++)
+		bodyarray[i]->LocalKinematics();
+	
+	Vect3 Torque; Torque.Zeros();
+	Vect3 Force; Force.Zeros();
+	
+	for(int i=numbodies-1;i>0;i--){
+		Torque(1)=FF(1,i);     
+		Torque(2)=FF(2,i);
+		Torque(3)=FF(3,i);		
+		Force(1)=FF(4,i);     
+		Force(2)=FF(5,i);
+		Force(3)=FF(6,i);   
+		bodyarray[i]->LocalTriangularization(Torque,Force);
+	}
+   	
+	for(int i=1;i<numbodies;i++){
+		bodyarray[i]->LocalForwardSubstitution();
+	}  
+}

lib/poems/onsolver.h

@@ -0,0 +1,51 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: onsolver.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef ONSOLVER_H
+#define ONSOLVER_H
+
+#include "solver.h"
+#include "onbody.h"
+#include <fstream>
+
+
+  
+class OnSolver : public Solver {
+       OnBody inertialframe;   
+       int numbodies;
+       OnBody** bodyarray;  
+       ColMatrix** q;		 
+       ColMatrix** qdot;
+		 ColMatrix** qdotdot;
+       ColMatrix** u;
+       ColMatrix** udot;
+       
+       
+       
+       void DeleteModel();
+       int CreateTopologyArray(int i, OnBody* body);
+       void CreateStateMatrixMaps();
+	    void GetType();
+public: 
+       OnSolver();
+       ~OnSolver();
+       void CreateModel();
+       void Solve(double time, Matrix& FF);       
+};
+
+#endif

lib/poems/particle.cpp

@@ -0,0 +1,40 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: particle.cpp                                            *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "particle.h"
+#include "fixedpoint.h"
+
+Particle::Particle(){
+}
+
+Particle::~Particle(){
+}
+
+BodyType Particle::GetType(){
+  return PARTICLE;
+}
+
+bool Particle::ReadInBodyData(std::istream& in){
+  in >> mass;
+  return true;
+}
+
+void Particle::WriteOutBodyData(std::ostream& out){
+  out << mass;
+}
+

lib/poems/particle.h

@@ -0,0 +1,34 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: particle.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef PARTICLE_H
+#define PARTICLE_H
+
+#include "body.h"
+
+
+class Particle : public Body  {
+public: 
+  Particle();
+  ~Particle();
+  BodyType GetType();
+  bool ReadInBodyData(std::istream& in);
+  void WriteOutBodyData(std::ostream& out);
+};
+
+#endif

lib/poems/poemslist.h

@@ -0,0 +1,228 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemslist.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef _POEMSLIST_H_
+#define _POEMSLIST_H_
+
+#include <iostream>
+
+using namespace std;
+
+template<class T> class ListElement{
+public:
+  ListElement<T>* prev;
+  ListElement<T>* next;
+  T* value;
+
+  ListElement();
+  ListElement(T* v);
+  ~ListElement();
+};
+
+template<class S> class List {
+  int numelements;
+  ListElement<S>* head;
+  ListElement<S>* tail;
+public:
+  List();
+  ~List();
+  int GetNumElements();
+  ListElement<S>* GetHeadElement();
+  ListElement<S>* GetTailElement();
+  void Remove(ListElement<S>* ele);
+  ListElement<S>* Append(S* v);
+  ListElement<S>* Prepend(S* v);
+  S** CreateArray();
+  S* operator()(int id);
+  void Append(List<S> * listToAppend);
+  void DeleteValues();
+  void RemoveElementAndDeleteValue(ListElement<S>* ele);  
+  void PrintList();
+};
+
+//
+// ListElement
+//
+
+template<class T> ListElement<T>::ListElement(){
+  next = prev = 0;
+  value = 0;
+};
+
+template<class T> ListElement<T>::ListElement(T* v){
+  next = prev = 0;
+  value = v;
+};
+
+template<class T> ListElement<T>::~ListElement(){
+};
+
+//
+// List
+//
+
+template<class S> List<S>::List(){
+  head = tail = 0;
+  numelements = 0;
+};
+
+template<class S> List<S>::~List(){
+  // delete all list elements
+
+  while(numelements)
+    Remove(tail);
+};
+
+template<class S> void List<S>::Append(List<S> * listToAppend)
+{
+	tail->next = listToAppend->head;
+	listToAppend->head->prev = tail;
+	tail = listToAppend->tail;
+};
+
+template<class S> int List<S>::GetNumElements(){
+  return numelements;
+};
+
+template<class S> ListElement<S>* List<S>::GetHeadElement(){
+  return head;
+};
+
+template<class S> ListElement<S>* List<S>::GetTailElement(){
+	return tail;
+};
+
+
+template<class S> void List<S>::Remove(ListElement<S>* ele){
+  if(!ele){
+    cerr << "ERROR: ListElement to be removed not defined" << endl;
+    exit(0);
+  }
+  if(!numelements){
+    cerr << "ERROR: List is empty" << endl;
+    exit(0);
+  }
+
+  if(ele != head)
+    ele->prev->next = ele->next;
+  else
+    head = ele->next;
+
+  if(ele != tail)
+    ele->next->prev = ele->prev;
+  else
+    tail = ele->prev;
+
+  numelements--;
+  if(ele)
+  delete ele;
+};
+
+template<class S> ListElement<S>* List<S>::Append(S* v){
+  if(!v){
+    cerr << "ERROR: cannot add null Body to list" << endl;
+    exit(0);
+  }
+
+  numelements++;
+  ListElement<S>* ele = new ListElement<S>(v);
+
+  if(numelements==1)
+    head = tail = ele;
+  else{
+	  /*
+    tail->next = ele;
+    ele->prev = tail;
+	 tail = ele;*/
+	  
+	  ele->prev = tail;
+	  tail = ele;	  
+	  ele->prev->next = ele;	  
+	  
+  }    
+  return ele;
+};
+
+template<class S> ListElement<S>* List<S>::Prepend(S* v){
+  if(!v){
+    cerr << "ERROR: cannot add null Body to list" << endl;
+    exit(0);
+  }
+
+  numelements++;
+  ListElement<S>* ele = new ListElement<S>(v);
+
+  if(numelements==1)
+    head = tail = ele;
+  else{
+	  ele->next = head;
+	  head = ele;
+	  ele->next->prev = ele;
+  }
+  return ele;
+};
+
+template<class S> S** List<S>::CreateArray(){
+  S** array = new S* [numelements];
+
+  ListElement<S>* ele = head;
+  for(int i=0;ele != 0;i++){
+    array[i] = ele->value;
+    ele = ele->next;
+  }
+  return array;
+};
+
+template<class S> S* List<S>::operator()(int id){
+  if(id >= numelements){
+    cerr << "ERROR: subscript out of bounds" << endl;
+    exit(0);
+  }
+  
+  ListElement<S>* ele = head;
+  for(int i=0;i<id;i++){
+    ele = ele->next;
+  }
+  
+  return ele->value;
+};
+
+template<class S> void List<S>::DeleteValues(){
+  while(numelements)
+    RemoveElementAndDeleteValue(tail);
+};
+
+template<class S> void List<S>::RemoveElementAndDeleteValue(ListElement<S>* ele){
+  S* v = ele->value;
+  Remove(ele);
+  delete v;
+};
+
+template<class S> void List<S>::PrintList(){
+	cout<<"Printing List "<<endl;
+	ListElement<S>* ele = head;
+	cout<<*(ele->value)<<" ";
+	ele = ele->next;
+	for(int k =2; k<numelements; k++){
+		cout<<*(ele->value)<<" ";	
+		ele = ele->next;
+	}
+	cout<<*(ele->value)<<endl;
+};
+
+
+#endif

lib/poems/poemsnodelib.h

@@ -0,0 +1,162 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemsnodelib.h                                          *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef NODELIB_H
+#define NODELIB_H
+
+#include <iostream>
+
+using namespace std;
+
+
+TreeNode *GetTreeNode(int item,TreeNode *lptr = NULL,TreeNode *rptr =NULL); 
+
+void FreeTreeNode(TreeNode *p);
+
+void Postorder(TreeNode *t, void visit(TreeNode* &t));
+
+void Preorder (TreeNode *t, void visit(TreeNode* &t));
+
+void CountLeaf (TreeNode *t, int& count);
+
+int Depth (TreeNode *t);
+
+void IndentBlanks(int num);
+
+void PrintTree (TreeNode *t, int level);
+
+
+// ---------------- Global functions-----------------//
+
+// postorder recursive scan of the nodes in a tree
+void Postorder (TreeNode *t, void visit(TreeNode* &t))
+{
+	// the recursive scan terminates on a empty subtree
+	if (t != NULL)
+	{
+		Postorder(t->Left(), visit);	// descend left
+		Postorder(t->Right(), visit);	// descend right
+		visit(t);					// visit the node
+	}
+}
+
+
+// preorder recursive scan of the nodes in a tree
+void Preorder (TreeNode *t, void visit(TreeNode* &t))
+{
+	// the recursive scan terminates on a empty subtree
+	if (t != NULL)
+	{
+		visit(t);				// visit the node
+		Preorder(t->Left(), visit);		// descend left
+		Preorder(t->Right(), visit);	// descend right			
+	}
+}
+
+
+//create TreeNode object with pointer fields lptr and rptr
+// The pointers have default value NULL
+TreeNode *GetTreeNode(int item,TreeNode *lptr,TreeNode *rptr)
+{
+	TreeNode *p;
+
+	// call new to allocate the new node
+	// pass parameters lptr and rptr to the function
+	p = new TreeNode(item, lptr, rptr);
+
+	// if insufficient memory, terminatewith an error message
+	if (p == NULL)
+	{
+		cerr << "Memory allocation failure!\n";
+		exit(1);
+	}
+
+	// return the pointer to the system generated memory
+	return p;
+}
+
+
+// deallocate dynamic memory associated with the node
+
+void FreeTreeNode(TreeNode *p)
+{
+	delete p;
+}
+
+
+// the function uses the postorder scan. a visit
+// tests whether the node is a leaf node
+void CountLeaf (TreeNode *t, int& count)
+{
+	//use postorder descent
+	if(t !=NULL)
+	{
+		CountLeaf(t->Left(), count); // descend left
+		CountLeaf(t->Right(), count); // descend right
+
+		// check if node t is a leaf node (no descendants)
+		// if so, increment the variable count
+		if (t->Left() == NULL && t->Right() == NULL)
+			count++;
+	}
+}
+
+
+// the function uses the postorder scan. it computes the
+// depth of the left and right subtrees of a node and 
+// returns the depth as 1 + max(depthLeft,depthRight).
+// the depth of an empty tree is -1
+int Depth (TreeNode *t)
+{
+	int depthLeft, depthRight, depthval;
+
+	if (t == NULL)
+		depthval = -1;
+	else
+	{
+		depthLeft = Depth(t->Left());
+		depthRight = Depth(t->Right());
+		depthval = 1+(depthLeft > depthRight?depthLeft:depthRight);
+	}
+	return depthval;
+}
+
+void IndentBlanks(int num)
+{
+//	const int indentblock = 6;
+
+	for(int i = 0; i < num; i++)
+		cout << " ";
+}
+
+void PrintTree (TreeNode *t, int level)
+{
+	//print tree with root t, as long as t!=NULL
+	if (t != NULL)
+	{
+		int indentUnit = 5;
+		// print right branch of tree t
+		PrintTree(t->Right(),level + 1);
+		// indent to current level; output node data
+		IndentBlanks(indentUnit*level);
+		cout << t->GetData() << endl;
+		// print left branch of tree t
+		PrintTree(t->Left(),level + 1);
+	}
+}
+#endif
+

lib/poems/poemsobject.cpp

@@ -0,0 +1,48 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemsobject.cpp                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#include "poemsobject.h"
+#include <string>
+
+POEMSObject::POEMSObject(){
+  name = 0;
+  ChangeName("unnamed");
+  ID = -1;
+}
+
+POEMSObject::~POEMSObject(){
+  delete [] name;
+}
+
+void POEMSObject::ChangeName(char* newname){
+  delete [] name;
+  name = new char[strlen(newname)+1];
+  strcpy(name,newname);
+}
+
+char* POEMSObject::GetName(){
+  return name;
+}
+
+int POEMSObject::GetID(){
+  return ID;
+}
+
+void POEMSObject::SetID(int id){
+  ID = id;
+}

lib/poems/poemsobject.h

@@ -0,0 +1,35 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemsobject.cpp                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#ifndef POEMSOBJECT_H
+#define POEMSOBJECT_H
+
+
+class POEMSObject {
+  char* name;
+  int ID;
+public: 
+  POEMSObject();
+  virtual ~POEMSObject();
+  void ChangeName(char* newname);
+  char* GetName();
+  int GetID();
+  void SetID(int id);
+};
+
+#endif

lib/poems/poemstree.h

@@ -0,0 +1,651 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemstree.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef TREE_H
+#define TREE_H
+
+#include "poemstreenode.h"
+#include "poemsnodelib.h"
+
+//tree.h
+//***********
+//***********
+// constants to indicate the balance factor of a node
+const int leftheavy = -1;
+const int balanced = 0;
+const int rightheavy = 1;
+
+
+
+class Tree{
+protected:
+	// pointer to tree root and node most recently accessed
+	TreeNode *root;
+	TreeNode *current;
+
+	// number of elements in the tree
+	int size;
+
+	//?????? the below are acuatually global functions right now
+	//memory allocation/deallocation
+	//TreeNode *GetTreeNode(const int& item,
+	//				TreeNode *lptr,TreeNode *rptr);
+	//void FreeTreeNode(TreeNode *p);
+
+	// used by the copy constructor and assignment operator
+	TreeNode *CopyTree(TreeNode *t);
+
+	// used by insert and delete method to re-establish
+	// the avl conditions after a node is added or deleted
+	// from a subtree
+	void SingleRotateLeft (TreeNode* &p);
+	void SingleRotateRight (TreeNode* &p);
+	void DoubleRotateLeft (TreeNode* &p);
+	void DoubleRotateRight (TreeNode* &p);
+	void UpdateLeftTree (TreeNode* &p, int &reviseBalanceFactor);
+	void UpdateRightTree (TreeNode* &p, int &reviseBalanceFactor);
+
+
+	// used by destructor, assignment operator and ClearList
+	void DeleteTree(TreeNode *t);
+	void ClearTree(TreeNode * &t);
+
+	// locate a node with data item and its parent in tree
+	// used by Find and Delete
+	TreeNode *FindNode(const int& item, TreeNode* & parent) const;
+
+public:
+	// constructor, destructor
+	Tree(void);
+	//Tree(const Tree& tree);  //????????need to write this
+	~Tree(void)
+	{
+		ClearTree(root);
+	}; //?????????????need to write this
+
+	// assignment operator
+	Tree& operator= (const Tree& rhs);
+
+	// standard list handling methods
+	int Find(int& item);
+	void * GetAuxData(int item) { return (void *)(FindNode(item, root)->GetAuxData());}
+	void Insert(const int& item, const int& data, void * AuxData = NULL);
+	void Delete(const int& item);
+	void AVLInsert(TreeNode* &tree, TreeNode* newNode, int &reviseBalanceFactor);
+	//void AVLDelete(TreeNode* &tree, TreeNode* newNode, int &reviseBalanceFactor);
+	void ClearList(void);
+	//int ListEmpty(void) const;
+	//int ListSize(void) const;
+
+	// tree specific methods
+	void Update(const int& item);
+	TreeNode *GetRoot(void) const;
+
+	//friend class DCASolver;
+};
+
+
+// constructor
+Tree::Tree(void)
+{
+	root = 0; // was NULL
+	current = 0; // was NULL
+	size = 0;
+}
+
+
+
+// return root pointer
+TreeNode *Tree::GetRoot(void) const
+{
+	return root;
+}
+
+
+// assignment operator
+Tree& Tree::operator = (const Tree& rhs)
+{
+	// can't copy a tree to itself
+	if (this == &rhs)
+		return *this;
+
+	// clear current tree. copy new tree into current object
+	ClearList();
+	root = CopyTree(rhs.root);
+
+	// assign current to root and set the tree size
+	current = root;
+	size = rhs.size;
+
+	// return reference to current object
+	return *this;
+}
+
+// search for data item in the tree. if found, return its node
+// address and a pointer to its parent; otherwise, return NULL
+TreeNode *Tree::FindNode(const int& item,
+								   TreeNode* & parent) const
+{
+	// cycle t through the tree starting with root
+	TreeNode *t = root;
+
+	// the parent of the root is NULL
+	parent = NULL;
+
+	// terminate on empty subtree
+	while(t != NULL)
+	{
+		// stop on a match
+		if (item == t->data)
+			break;
+		else
+		{
+			// update the parent pointer and move right of left
+			parent = t;
+			if (item < t->data)
+				t = t->left;
+			else
+				t = t->right;
+		}
+	}
+
+	// return pointer to node; NULL if not found
+	return t;
+}
+
+// search for item. if found, assign the node data to item
+int Tree::Find(int& item)
+{
+	// we use FindNode, which requires a parent parameter
+	TreeNode *parent;
+
+	// search tree for item. assign matching node to current
+	current = FindNode (item, parent);
+
+	// if item found, assign data to item and return True
+	if (current != NULL)
+	{
+		item = current->data;
+		return (int)current->GetAuxData();
+	}
+	else
+		// item not found in the tree. return False
+		return 0;
+}
+
+
+void Tree::Insert(const int& item, const int& data, void * AuxData)
+{
+	// declare AVL tree node pointer; using base class method
+	// GetRoot. cast to larger node and assign root pointer
+	TreeNode *treeRoot, *newNode;
+	 treeRoot = GetRoot();
+
+	// flag used by AVLInsert to rebalance nodes
+	int reviseBalanceFactor = 0;
+
+	// get a new AVL tree node with empty pointer fields
+	newNode = GetTreeNode(item,NULL,NULL);
+	newNode->data = data;
+	newNode->SetAuxData(AuxData);
+	// call recursive routine to actually insert the element
+	AVLInsert(treeRoot, newNode, reviseBalanceFactor);
+
+	// assign new values to data members in the base class
+	root = treeRoot;
+	current = newNode;
+	size++;
+
+
+
+
+
+	////the below is for an unbalance insert algorithm
+	/*
+	// t is the current node in transversal, parent the pervios node
+	TreeNode *t = root, *parent = NULL, *newNode;
+
+	// terminate on empty subtree
+	while(t != NULL)
+	{
+		// update the parent pointer. then go left or right
+		parent = t;
+		if (item < t->data)
+			t = t->left;
+		else
+			t = t->right;
+	}
+
+	// create the new leaf node
+	newNode = GetTreeNode(item,NULL,NULL);
+
+	// if parent is NULL, insert as a root node
+	if (parent == NULL)
+		root = newNode;
+
+	// if item < parent->data. insert as left child
+	else if  (item < parent->data)
+		parent->left = newNode;
+
+	else
+	// if item >= parent->data. insert as right child
+	parent->right = newNode;
+	// assign current as address of new node and increment size
+	current = newNode;
+	size++;
+	*/
+}
+
+void Tree::AVLInsert(TreeNode *&tree, TreeNode *newNode, int &reviseBalanceFactor)
+{
+	// flag indicates change node's balanceFactor will occur
+	int rebalanceCurrNode;
+
+	// scan reaches an empty tree; time to insert the new node
+	if (tree == NULL)
+	{
+		// update the parent to point at newNode
+		tree = newNode;
+
+		// assign balanceFactor = 0 to new node
+		tree->balanceFactor = balanced;
+		// broadcast message; balanceFactor value is modified
+		reviseBalanceFactor = 1;
+	}
+	// recursively move left if new data < current data
+	else if (newNode->data < tree->data)
+	{
+		AVLInsert(tree->left,newNode,rebalanceCurrNode);
+		// check if balanceFactor must be updated.
+		if (rebalanceCurrNode)
+		{
+			// went left from node that is left heavy. will
+			// violate AVL condition; use rotation (case 3)
+			if (tree->balanceFactor == leftheavy)
+				UpdateLeftTree(tree,reviseBalanceFactor);
+
+			// went left from balanced node. will create
+			// node left on the left. AVL condition OK (case 1)
+			else if (tree->balanceFactor == balanced)
+			{
+				tree->balanceFactor = leftheavy;
+				reviseBalanceFactor = 1;
+			}
+			// went left from node that is right heavy. will
+			// balance the node. AVL condition OK (case 2)
+			else
+			{
+				tree->balanceFactor = balanced;
+				reviseBalanceFactor = 0;
+			}
+		}
+        else 
+            // no balancing occurs; do not ask previous nodes
+			reviseBalanceFactor = 0;
+	}
+    // otherwise recursively move right
+    else
+	{
+		AVLInsert(tree->right, newNode, rebalanceCurrNode);
+		// check if balanceFactor must be updated.
+		if (rebalanceCurrNode)
+		{
+			// went right from node that is left heavy. wil;
+			// balance the node. AVL condition OK (case 2)
+			if (tree->balanceFactor == leftheavy)
+			{
+				// scanning right subtree. node heavy on left.
+				// the node will become balanced
+				tree->balanceFactor = balanced;
+				reviseBalanceFactor = 0;
+			}
+			// went right from balanced node. will create
+			// node heavy on the right. AVL condition OK (case 1)
+			else if (tree->balanceFactor == balanced)
+			{
+				// node is balanced; will become heavy on right
+				tree->balanceFactor = rightheavy;
+				reviseBalanceFactor = 1;
+			}
+			// went right from node that is right heavy. will
+			// violate AVL condition; use rotation (case 3)
+			else
+				UpdateRightTree(tree, reviseBalanceFactor);
+		}
+		else
+			reviseBalanceFactor = 0;
+	}
+}
+
+
+void Tree::UpdateLeftTree (TreeNode* &p, int &reviseBalanceFactor)
+{
+	TreeNode *lc;
+
+	lc = p->Left();			// left subtree is also heavy
+	if (lc->balanceFactor == leftheavy)
+	{
+		SingleRotateRight(p);
+		reviseBalanceFactor = 0;
+	}
+	// is right subtree heavy?
+	else if (lc->balanceFactor == rightheavy)
+	{
+		// make a double rotation
+		DoubleRotateRight(p);
+		// root is now balance
+		reviseBalanceFactor = 0;
+	}
+}
+
+void Tree::UpdateRightTree (TreeNode* &p, int &reviseBalanceFactor)
+{
+	TreeNode *lc;
+
+	lc = p->Right();			// right subtree is also heavy
+	if (lc->balanceFactor == rightheavy)
+	{
+		SingleRotateLeft(p);
+		reviseBalanceFactor = 0;
+	}
+	// is left subtree heavy?
+	else if (lc->balanceFactor == leftheavy)
+	{
+		// make a double rotation
+		DoubleRotateLeft(p);
+		// root is now balance
+		reviseBalanceFactor = 0;
+	}
+}
+
+void Tree::SingleRotateRight (TreeNode* &p)
+{
+	// the left subtree of p is heavy
+	TreeNode *lc;
+
+	// assign the left subtree to lc
+	lc = p->Left();
+
+	// update the balance factor for parent and left child
+	p->balanceFactor = balanced;
+	lc->balanceFactor = balanced;
+
+	// any right subtree st of lc must continue as right
+	// subtree of lc. do by making it a left subtree of p
+	p->left = lc->Right();
+
+	// rotate p (larger node) into right subtree of lc
+	// make lc the pivot node
+	lc->right = p;
+	p = lc;
+}
+
+void Tree::SingleRotateLeft (TreeNode* &p)
+{
+	// the right subtree of p is heavy
+	TreeNode *lc;
+
+	// assign the left subtree to lc
+	lc = p->Right();
+
+	// update the balance factor for parent and left child
+	p->balanceFactor = balanced;
+	lc->balanceFactor = balanced;
+
+	// any right subtree st of lc must continue as right
+	// subtree of lc. do by making it a left subtree of p
+	p->right = lc->Left();
+
+	// rotate p (larger node) into right subtree of lc
+	// make lc the pivot node
+	lc->left = p;
+	p = lc;
+}
+
+// double rotation right about node p
+void Tree::DoubleRotateRight (TreeNode* &p)
+{
+	// two subtrees that are rotated
+	TreeNode *lc, *np;
+
+	// in the tree, node(lc) <= node(np) < node(p)
+	lc = p->Left();			// lc is left child of parent
+	np = lc->Right();		// np is right child of lc
+	
+	// update balance factors for p, lc, and np
+	if (np->balanceFactor == rightheavy)
+	{
+		p->balanceFactor = balanced;
+		lc->balanceFactor = rightheavy;
+	}
+	else if (np->balanceFactor == balanced)
+	{
+		p->balanceFactor = balanced;
+		lc->balanceFactor = balanced;
+	}
+	else
+	{
+		p->balanceFactor = rightheavy;
+		lc->balanceFactor = balanced;
+	}	
+	np->balanceFactor = balanced;
+
+	// before np replaces the parent p, take care of subtrees
+	// detach old children and attach new children
+	lc->right = np->Left();
+	np->left = lc;
+	p->left = np->Right();
+	np->right = p;
+	p = np;
+}
+
+void Tree::DoubleRotateLeft (TreeNode* &p)
+{
+	// two subtrees that are rotated
+	TreeNode *lc, *np;
+
+	// in the tree, node(lc) <= node(np) < node(p)
+	lc = p->Right();			// lc is right child of parent
+	np = lc->Left();		// np is left child of lc
+	
+	// update balance factors for p, lc, and np
+	if (np->balanceFactor == leftheavy)
+	{
+		p->balanceFactor = balanced;
+		lc->balanceFactor = leftheavy;
+	}
+	else if (np->balanceFactor == balanced)
+	{
+		p->balanceFactor = balanced;
+		lc->balanceFactor = balanced;
+	}
+	else
+	{
+		p->balanceFactor = leftheavy;
+		lc->balanceFactor = balanced;
+	}	
+	np->balanceFactor = balanced;
+
+	// before np replaces the parent p, take care of subtrees
+	// detach old children and attach new children
+	lc->left = np->Right();
+	np->right = lc;
+	p->right = np->Left();
+	np->left = p;
+	p = np;
+}
+
+// if item is in the tree, delete it
+void Tree::Delete(const int& item)
+{
+	// DNodePtr = pointer to node D that is deleted
+	// PNodePtr = pointer to parent P of node D
+	// RNodePtr = pointer to node R that replaces D
+	TreeNode *DNodePtr, *PNodePtr, *RNodePtr;
+
+	// search for a node containing data value item. obtain its
+	// node adress and that of its parent
+	if ((DNodePtr = FindNode (item, PNodePtr)) == NULL)
+		return;
+
+	// If D has NULL pointer, the 
+	// replacement node is the one on the other branch
+	if (DNodePtr->right == NULL)
+		RNodePtr = DNodePtr->left;
+	else if (DNodePtr->left == NULL)
+		RNodePtr = DNodePtr->right;
+	// Both pointers of DNodePtr are non-NULL
+	else
+	{
+		// Find and unlink replacement node for D
+		// Starting on the left branch of node D,
+		// find node whose data value is the largest of all
+		// nodes whose values are less than the value in D
+		// Unlink the node from the tree
+
+		// PofRNodePtr = pointer to parent of replacement node
+		TreeNode *PofRNodePtr = DNodePtr;
+
+		// frist possible replacement is left child D
+		RNodePtr = DNodePtr->left;
+
+		// descend down right subtree of the left child of D
+		// keeping a record of current node and its parent.
+		// when we stop, we have found the replacement
+		while (RNodePtr->right != NULL)
+		{
+			PofRNodePtr = RNodePtr;
+			RNodePtr = RNodePtr;
+		}
+
+		if (PofRNodePtr == DNodePtr)
+			// left child of deleted node is the replacement
+			// assign right subtree of D to R
+			RNodePtr->right =  DNodePtr->right;
+		else
+		{
+			// we moved at least one node down a right brance
+			// delete replacement node from tree by assigning
+			// its left branc to its parent
+			PofRNodePtr->right = RNodePtr->left;
+
+			// put replacement node in place of DNodePtr.
+			RNodePtr->left = DNodePtr->left;
+			RNodePtr->right = DNodePtr->right;
+		}
+	}
+
+	// complete the link to the parent node
+	// deleting the root node. assign new root
+	if (PNodePtr == NULL)
+		root = RNodePtr;
+	// attach R to the correct branch of P
+	else if (DNodePtr->data < PNodePtr->data)
+		PNodePtr->left = RNodePtr;
+	else
+		PNodePtr->right = RNodePtr;
+
+	// delete the node from memory and decrement list size
+	FreeTreeNode(DNodePtr);  // this says FirstTreeNode in the book, should be a typo
+	size--;
+}
+
+
+
+
+
+// if current node is defined and its data value matches item,
+// assign node value to item; otherwise, insert item in tree
+void Tree::Update(const int& item)
+{
+	if (current !=NULL && current->data == item)
+		current->data = item;
+	else
+		Insert(item, item);
+}
+
+// create duplicate of tree t; return the new root
+TreeNode *Tree::CopyTree(TreeNode *t)
+{
+	// variable newnode points at each new node that is
+	// created by a call to GetTreeNode and later attached to
+	// the new tree. newlptr and newrptr point to the child of
+	// newnode and are passed as parameters to GetTreeNode
+	TreeNode *newlptr, *newrptr, *newnode;
+
+	// stop the recursive scan when we arrive at an empty tree
+	if (t == NULL)
+		return NULL;
+
+	// CopyTree builds a new tree by scanning the nodes of t.
+	// At each node in t, CopyTree checks for a left child. if
+	// present it makes a copy of left child or returns NULL.
+	// the algorithm similarly checks for a right child.
+	// CopyTree builds a copy of node using GetTreeNode and
+	// appends copy of left and right children to node.
+
+	if (t->Left() !=NULL)
+		newlptr = CopyTree(t->Left());
+	else
+		newlptr = NULL;
+
+	if (t->Right() !=NULL)
+		newrptr = CopyTree(t->Right());
+	else
+		newrptr = NULL;
+
+
+	// Build new tree from the bottom up by building the two
+	// children and then building the parent
+	newnode = GetTreeNode(t->data, newlptr, newrptr);
+
+	// return a pointer to the newly created node
+	return newnode;
+}
+
+
+// us the postorder scanning algorithm to traverse the nodes in
+// the tree and delete each node as the vist operation
+void Tree::DeleteTree(TreeNode *t)
+{
+	if (t != NULL)
+	{
+		DeleteTree(t->Left());
+		DeleteTree(t->Right());
+		if (t->GetAuxData() != NULL)
+					delete t->GetAuxData();
+		FreeTreeNode(t);
+	}
+}
+
+// call the function DeleteTree to deallocate the nodes. then
+// set the root pointer back to NULL
+void Tree::ClearTree(TreeNode * &t)
+{
+	DeleteTree(t);
+	t = NULL;		// root now NULL
+}
+
+// delete all nodes in list
+void Tree::ClearList(void)
+{
+	delete root;
+	delete current;
+	size = 0;
+}
+
+#endif

lib/poems/poemstreenode.cpp

@@ -0,0 +1,49 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemstreenode.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "poemstreenode.h"
+
+// constructor; initialize the data and pointer fields
+// The pointer value NULL assigns a empty subtree
+TreeNode::TreeNode (const int & item, TreeNode *lptr,TreeNode *rptr,
+					int balfac):data(item), left(lptr), right(rptr), balanceFactor(balfac)
+{
+}
+
+
+
+// return left
+TreeNode* TreeNode::Left(void)
+{
+	return left;
+}
+
+// return right
+TreeNode* TreeNode::Right(void)
+{
+	return right;
+}
+
+int TreeNode::GetBalanceFactor(void)
+{
+	return balanceFactor;
+}
+
+int TreeNode::GetData(void)
+{
+	return data;
+}

lib/poems/poemstreenode.h

@@ -0,0 +1,52 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: poemstreenode.h                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef TREENODE_H
+#define TREENODE_H
+
+//#define NULL 0
+
+
+//Tree depends on TreeNode
+class Tree;
+
+// declares a tree node object for a binary tree
+class TreeNode{
+
+private:
+// points to the left and right children of the node
+	TreeNode *left;
+	TreeNode *right;
+
+	int balanceFactor;
+	int data;
+	void * aux_data;
+public:	
+	// make Tree a friend because it needs access to left and right pointer fields of a node
+	friend class Tree;
+	TreeNode * Left();
+	TreeNode * Right();
+	int GetData();
+	void * GetAuxData() {return aux_data;};
+	void SetAuxData(void * AuxData) {aux_data = AuxData;};
+	int GetBalanceFactor();
+	TreeNode(const int &item, TreeNode *lptr, TreeNode *rptr, int balfac = 0);
+	//friend class DCASolver;
+};
+
+#endif
+

lib/poems/point.cpp

@@ -0,0 +1,44 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: point.cpp                                                  *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "points.h"
+
+Point::Point(){
+  position.Zeros();
+}
+Point::~Point(){
+}
+
+bool Point::ReadIn(std::istream& in){
+  return ReadInPointData(in);
+}
+
+void Point::WriteOut(std::ostream& out){
+  out << GetType() << ' ' << GetName() << ' ';
+  WriteOutPointData(out);
+}
+
+Point* NewPoint(int type){
+  switch( PointType(type) )
+  {
+    case FIXEDPOINT :  // A Fixed Point
+      return new FixedPoint();
+    default  :  // error
+      return 0;
+  }
+}

lib/poems/point.h

@@ -0,0 +1,49 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: point.h                                                  *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef POINT_H
+#define POINT_H
+
+#include <iostream>
+#include "poemsobject.h"
+#include "vect3.h"
+
+
+// emumerated type
+enum PointType {
+  FIXEDPOINT = 0
+};
+
+class Point : public POEMSObject {
+public: 
+  Vect3 position;
+
+  Point();   
+  bool ReadIn(std::istream& in);  
+  void WriteOut(std::ostream& out);
+  
+  virtual ~Point();  
+  virtual PointType GetType() = 0;
+  virtual Vect3 GetPoint() = 0;
+  virtual bool ReadInPointData(std::istream& in) = 0;
+  virtual void WriteOutPointData(std::ostream& out) = 0;
+};
+
+// global point functions
+Point* NewPoint(int type);
+
+#endif

lib/poems/points.h

@@ -0,0 +1,24 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: points.h		                                          *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef POINTS_H
+#define POINTS_H
+
+#include "point.h"
+#include "fixedpoint.h"
+
+#endif

lib/poems/prismaticjoint.cpp

@@ -0,0 +1,194 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: prismaticjoint.cpp                                      *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "prismaticjoint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+
+PrismaticJoint::PrismaticJoint(){
+  q.Dim(1);
+  qdot.Dim(1);
+  u.Dim(1);
+  udot.Dim(1);
+}
+PrismaticJoint::~PrismaticJoint(){
+}
+
+JointType PrismaticJoint::GetType(){
+  return PRISMATICJOINT;
+}
+
+bool PrismaticJoint::ReadInJointData(std::istream& in){
+  in >> axis_pk;
+  axis_k = T(pk_C_ko)*axis_pk;
+
+  // init the constant transforms
+  pk_C_k = pk_C_ko;
+  k_C_pk = T(pk_C_k);
+    
+  return true;
+}
+
+void PrismaticJoint::WriteOutJointData(std::ostream& out){
+  out << axis_pk;
+}
+
+Matrix PrismaticJoint::GetForward_sP(){
+  Vect3 zero;
+  zero.Zeros();
+
+  // sP = [zero;axis]
+  return Stack(zero,axis_k);
+}
+
+void PrismaticJoint::UpdateForward_sP( Matrix& sP){
+  // sP is constant, do nothing.
+}
+
+Matrix PrismaticJoint::GetBackward_sP(){
+  Vect3 zero;
+  zero.Zeros();
+
+  // sP = [zero;axis]
+  return -Stack(zero,axis_pk);
+}
+
+void PrismaticJoint::UpdateBackward_sP( Matrix& sP){
+  // sP is constant, do nothing.
+}
+
+void PrismaticJoint::ComputeForwardTransforms(){
+  ComputeForwardGlobalTransform();
+}
+
+void PrismaticJoint::ComputeBackwardTransforms(){
+  ComputeBackwardGlobalTransform();
+}
+
+void PrismaticJoint::ComputeLocalTransform(){
+  // the transform is constant, do nothing
+}
+
+void PrismaticJoint::ForwardKinematics(){
+  Vect3 result1,result2,result3;
+  Vect3 d_pk;
+
+  // orientations
+  ComputeForwardTransforms();
+
+  // compute position vector r12
+  //r12 = point1->position + axis_pk * q - pk_C_k * point2->position;
+  FastMult(pk_C_k,point2->position,result1);
+  FastMult(q.BasicGet(0),axis_pk,d_pk);
+  FastTripleSumPPM(point1->position,d_pk,result1,r12);
+
+  // compute position vector r21
+  FastNegMult(k_C_pk,r12,r21);
+  
+  // compute global location
+  // body2->r = body1->r + body1->n_C_k * r12;
+  FastMult(body1->n_C_k,r12,result1);
+  FastAdd(body1->r,result1,body2->r);
+
+  // compute qdot (for Prismatic joint qdot = u)
+  // qdot = u
+  FastAssign(u,qdot);
+
+  // angular velocities
+  //body2->omega = body1->omega;
+  //body2->omega_k = T(pk_C_k) * body1->omega_k;
+  FastAssign(body1->omega,body2->omega);
+  FastMult(k_C_pk,body1->omega_k,body2->omega_k);
+  
+  // compute velocities
+  Vect3 pk_v_k;
+  Vect3 wxgamma;
+  FastMult(u.BasicGet(0),axis_k,pk_v_k);
+  FastMult(k_C_pk,body1->v_k,result1);
+  FastCross(body2->omega_k,r12,wxgamma);
+  FastTripleSum(result1,pk_v_k,wxgamma,body2->v_k);
+  FastMult(body2->n_C_k,body2->v_k,body2->v);
+
+  // compute state explicit angular acceleration
+  FastMult(k_C_pk,body1->alpha_t,body2->alpha_t);
+
+  // compute state explicit acceleration
+  FastCross(r21,body1->alpha_t,result1);
+  FastAdd(body1->a_t,result1,result2);
+  FastMult(k_C_pk,result2,result1);
+
+  FastCross(body2->omega_k,pk_v_k,result2);
+  FastMult(2.0,result2,result3);
+  FastCross(body2->omega_k,wxgamma,result2);
+  FastTripleSum(result1,result2,result3,body2->a_t);
+}
+
+void PrismaticJoint::BackwardKinematics(){
+  Vect3 result1,result2,result3;
+  Vect3 d_k;
+
+  // orientations
+  ComputeBackwardTransforms();
+
+  // compute position vector r21
+  //r21 = point2->position + axis_k * q - k_C_pk * point1->position;
+  FastMult(k_C_pk,point1->position,result1);
+  FastMult(-q.BasicGet(0),axis_k,d_k);
+  FastTripleSumPPM(point2->position,d_k,result1,r21);
+
+  // compute position vector r12
+  FastNegMult(pk_C_k,r21,r12);
+
+  // compute global location
+  // body1->r = body2->r + body2->n_C_k * r21;
+  FastMult(body2->n_C_k,r21,result1);
+  FastAdd(body2->r,result1,body1->r);
+
+  // compute qdot (for Prismatic joint qdot = u)
+  // qdot = u
+  FastAssign(u,qdot);
+
+  // angular velocities
+  //body1->omega = body2->omega;
+  //body1->omega_k = pk_C_k * body2->omega_k;
+  FastAssign(body2->omega,body1->omega);
+  FastMult(pk_C_k,body2->omega_k,body1->omega_k);
+
+  // compute velocities
+  Vect3 k_v_pk;
+  Vect3 wxgamma;
+  FastMult(-u.BasicGet(0),axis_pk,k_v_pk);
+  FastMult(pk_C_k,body2->v_k,result1);
+  FastCross(body1->omega_k,r21,wxgamma);
+  FastTripleSum(result1,k_v_pk,wxgamma,body1->v_k);
+  FastMult(body1->n_C_k,body1->v_k,body1->v);
+
+  // compute state explicit angular acceleration
+  FastMult(pk_C_k,body2->alpha_t,body1->alpha_t);
+
+  // compute state explicit acceleration
+  FastCross(r12,body2->alpha_t,result1);
+  FastAdd(body2->a_t,result1,result2);
+  FastMult(pk_C_k,result2,result1);
+
+  FastCross(body1->omega_k,k_v_pk,result2);
+  FastMult(2.0,result2,result3);
+  FastCross(body1->omega_k,wxgamma,result2);
+  FastTripleSum(result1,result2,result3,body1->a_t);
+}

lib/poems/prismaticjoint.h

@@ -0,0 +1,47 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: prismaticjoint.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef PRISMATICJOINT_H
+#define PRISMATICJOINT_H
+
+#include "joint.h"
+#include "vect3.h"
+#include "mat3x3.h"
+
+
+
+class PrismaticJoint : public Joint  {
+  Vect3 axis_pk; // unit vector in body1 basis
+  Vect3 axis_k;  // unit vector in body2 basis
+public:
+  PrismaticJoint();
+  ~PrismaticJoint();
+  JointType GetType();
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ComputeForwardTransforms();
+  void ComputeBackwardTransforms();
+  void ComputeLocalTransform();
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/revolutejoint.cpp

@@ -0,0 +1,219 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: revolutejoint.cpp                                        *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "revolutejoint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+
+RevoluteJoint::RevoluteJoint(){
+  DimQandU(1);
+  Vect3 axis;
+  axis.Zeros();
+  axis(3) = 1;
+  SetAxisPK(axis);
+}
+
+RevoluteJoint::~RevoluteJoint(){
+}
+
+JointType RevoluteJoint::GetType(){
+  return REVOLUTEJOINT;
+}
+
+void RevoluteJoint::SetAxisK(VirtualMatrix& axis){
+  axis_k = axis;
+  axis_pk = pk_C_ko*axis_k;
+}
+
+void RevoluteJoint::SetAxisPK(VirtualMatrix& axis){
+  axis_pk = axis;
+  axis_k = T(pk_C_ko)*axis_pk;
+}
+
+bool RevoluteJoint::ReadInJointData(std::istream& in){
+  Vect3 axis;
+  in >> axis;
+  SetAxisPK(axis);
+  return true;
+}
+
+void RevoluteJoint::WriteOutJointData(std::ostream& out){
+  out << axis_pk;
+}
+
+Matrix RevoluteJoint::GetForward_sP(){
+  Vect3 v_kk;
+
+  // v_kk = axis x r
+  FastCross(point2->position,axis_k,v_kk);
+
+  // sP = [axis;v_kk]
+  return Stack(axis_k,v_kk);
+}
+
+void RevoluteJoint::UpdateForward_sP( Matrix& sP){
+  // sP is constant, do nothing.
+}
+
+Matrix RevoluteJoint::GetBackward_sP(){
+  Vect3 v_kk;
+
+  // v_kk = axis x r
+  FastCross(point1->position,axis_pk,v_kk);
+
+  // sP = [axis;v_kk]
+  return -Stack(axis_pk,v_kk);;
+}
+
+void RevoluteJoint::UpdateBackward_sP( Matrix& sP){
+  // sP is constant, do nothing.
+}
+
+void RevoluteJoint::ComputeLocalTransform(){
+  Mat3x3 ko_C_k;
+  FastSimpleRotation(axis_k,q.BasicGet(0),ko_C_k);
+  // pk_C_k = pk_C_ko * ko_C_k;
+  FastMult(pk_C_ko,ko_C_k,pk_C_k);
+}
+
+void RevoluteJoint::ForwardKinematics(){
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 pk_w_k;
+
+  // orientations
+  ComputeForwardTransforms();
+
+  // compute position vector r12
+  //r12 = point1->position - pk_C_k * point2->position;
+  FastMult(pk_C_k,point2->position,result1); 
+  FastSubt(point1->position,result1,r12);// Jacks comment: needs flipping!!!
+
+  // compute position vector r21
+  FastNegMult(k_C_pk,r12,r21);
+
+  // compute global location
+  // body2->r = body1->r + body1->n_C_k * r12;
+  FastMult(body1->n_C_k,r12,result1);
+  FastAdd(body1->r,result1,body2->r);
+
+  // compute qdot (for revolute joint qdot = u)
+  // qdot = u
+  FastAssign(u,qdot);
+
+  // angular velocities
+  //body2->omega = body1->omega + body1->n_C_k * axis_pk * u;
+  //pk_w_k = axis_k * u;
+  //body2->omega_k = T(pk_C_k) * body1->omega_k + pk_w_k;
+  double u_double = u.BasicGet(0);
+  FastMult(u_double,axis_pk,result1);
+  FastMult(body1->n_C_k,result1,result2);
+  FastAdd(body1->omega,result2,body2->omega);
+  FastMult(u_double,axis_k,pk_w_k);
+  FastTMult(pk_C_k,body1->omega_k,result1);
+  FastAdd(result1,pk_w_k,body2->omega_k);
+
+  // compute velocities
+  FastCross(body1->omega_k,r12,result1);
+  FastCross(point2->position,pk_w_k,result2);
+  FastAdd(body1->v_k,result1,result3);
+  FastTMult(pk_C_k,result3,result4);
+  FastAdd(result2,result4,body2->v_k);
+  FastMult(body2->n_C_k,body2->v_k,body2->v);
+
+  // compute state explicit angular acceleration
+  FastCross(body2->omega_k,pk_w_k,result1);
+  FastTMult(pk_C_k,body1->alpha_t,result2);
+  FastAdd(result1,result2,body2->alpha_t);
+
+  // compute state explicit acceleration
+  FastCross(body1->alpha_t,point1->position,result1);
+  FastCross(body1->omega_k,point1->position,result2);
+  FastCross(body1->omega_k,result2,result3);
+  FastTripleSum(body1->a_t,result1,result3,result4);
+  FastTMult(pk_C_k,result4,result5);
+
+  FastCross(point2->position,body2->alpha_t,result1);
+  FastCross(point2->position,body2->omega_k,result2);
+  FastCross(body2->omega_k,result2,result3);
+
+  FastTripleSum(result5,result1,result3,body2->a_t);
+}
+
+void RevoluteJoint::BackwardKinematics(){
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 k_w_pk;
+
+  // orientations
+  ComputeBackwardTransforms();
+
+  // compute position vector r21
+  //r21 = point2->position - k_C_pk * point1->position;
+  FastMult(k_C_pk,point1->position,result1);
+  FastSubt(point2->position,result1,r21);
+
+  // compute position vector r21
+  FastNegMult(pk_C_k,r21,r12);
+
+  // compute global location
+  // body1->r = body2->r + body2->n_C_k * r21;
+  FastMult(body2->n_C_k,r21,result1);
+  FastAdd(body2->r,result1,body1->r);
+
+  // compute qdot (for revolute joint qdot = u)
+  // qdot = u
+  FastAssign(u,qdot);
+
+  // angular velocities
+  //body1->omega = body2->omega - body2->n_C_k * axis_k * u;
+  //k_w_pk = - axis_pk * u;
+  //body1->omega_k = pk_C_k * body2->omega_k + k_w_pk;
+  double u_double = u.BasicGet(0);
+  FastMult(-u_double,axis_k,result1);
+  FastMult(body2->n_C_k,result1,result2);
+  FastAdd(body2->omega,result2,body1->omega);
+  FastMult(-u_double,axis_pk,k_w_pk);
+  FastMult(pk_C_k,body2->omega_k,result1);
+  FastAdd(result1,k_w_pk,body1->omega_k);
+
+  // compute velocities
+  FastCross(body2->omega_k,r21,result1);
+  FastCross(point1->position,k_w_pk,result2);
+  FastAdd(body2->v_k,result1,result3);
+  FastMult(pk_C_k,result3,result4);
+  FastAdd(result2,result4,body1->v_k);
+  FastMult(body1->n_C_k,body1->v_k,body1->v);
+
+  // compute state explicit angular acceleration
+  FastCross(body1->omega_k,k_w_pk,result1);
+  FastMult(pk_C_k,body2->alpha_t,result2);
+  FastAdd(result1,result2,body1->alpha_t);
+
+  // compute state explicit acceleration
+  FastCross(body2->alpha_t,point2->position,result1);
+  FastCross(body2->omega_k,point2->position,result2);
+  FastCross(body2->omega_k,result2,result3);
+  FastTripleSum(body2->a_t,result1,result3,result4);
+  FastMult(pk_C_k,result4,result5);
+
+  FastCross(point1->position,body1->alpha_t,result1);
+  FastCross(point1->position,body1->omega_k,result2);
+  FastCross(body1->omega_k,result2,result3);
+
+  FastTripleSum(result5,result1,result3,body1->a_t);
+}

lib/poems/revolutejoint.h

@@ -0,0 +1,47 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: revolutejoint.h                                         *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef REVOLUTEJOINT_H
+#define REVOLUTEJOINT_H
+
+#include "joint.h"
+#include "vect3.h"
+#include "mat3x3.h"
+
+
+
+class RevoluteJoint : public Joint  {
+  Vect3 axis_pk; // unit vector in body1 basis
+  Vect3 axis_k;  // unit vector in body2 basis
+public: 
+  RevoluteJoint();
+  ~RevoluteJoint();
+  JointType GetType();
+  void SetAxisK(VirtualMatrix& axis);
+  void SetAxisPK(VirtualMatrix& axis);
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ComputeLocalTransform();
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/rigidbody.cpp

@@ -0,0 +1,39 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: rigidbody.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "rigidbody.h"
+#include "fixedpoint.h"
+
+using namespace std;
+
+RigidBody::RigidBody(){
+}
+RigidBody::~RigidBody(){
+}
+
+BodyType RigidBody::GetType(){
+  return RIGIDBODY;
+}
+
+bool RigidBody::ReadInBodyData(istream& in){
+  in >> mass >> inertia;
+  return true;
+}
+
+void RigidBody::WriteOutBodyData(ostream& out){
+  out << mass << ' ' << inertia;
+}

lib/poems/rigidbody.h

@@ -0,0 +1,32 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: rigidbody.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef RIGIDBODY_H
+#define RIGIDBODY_H
+
+#include "body.h"
+
+class RigidBody : public Body  {
+public: 
+  RigidBody();
+  ~RigidBody();
+  BodyType GetType();
+  bool ReadInBodyData(std::istream& in);
+  void WriteOutBodyData(std::ostream& out);
+};
+
+#endif

lib/poems/rowmatrix.cpp

@@ -0,0 +1,175 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: rowmatrix.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "rowmatrix.h"
+#include "colmatrix.h"
+#include <iostream>
+
+using namespace std;
+
+RowMatrix::RowMatrix(){
+  numcols = 0;
+  elements = 0;
+}
+
+RowMatrix::~RowMatrix(){
+  delete [] elements;
+}
+
+RowMatrix::RowMatrix(const RowMatrix& A){  // copy constructor
+  numcols = 0;
+  elements = 0;
+  Dim(A.numcols);
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.elements[i];
+}
+
+RowMatrix::RowMatrix(const VirtualRowMatrix& A){  // copy constructor
+  numcols = 0;
+  elements = 0;
+  Dim(A.GetNumCols());
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.BasicGet(i);
+}
+
+RowMatrix::RowMatrix(const VirtualMatrix& A){  // copy constructor
+  if( A.GetNumRows() != 1 ){
+    cerr << "error trying to write a 2D matrix to a collumn" << endl;
+    exit(1);
+  }
+  numcols = 0;
+  elements = 0;
+  Dim(A.GetNumCols());
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+RowMatrix::RowMatrix(int n){  // size constructor
+  numcols = 0;
+  elements = 0;
+  Dim(n);
+}
+
+void RowMatrix::Dim(int n){
+  delete [] elements;
+  numcols = n;
+  elements = new double [n];
+}
+
+void RowMatrix::Const(double value){
+  for(int i=0;i<numcols;i++)
+    elements[i] = value;
+}
+
+double& RowMatrix::operator_1int (int i){ // array access
+  if((i>numcols) || (i<1)){
+    cerr << "matrix index invalid in operator ()" << endl;
+    exit(1);
+  }
+  return elements[i-1];
+}
+
+double RowMatrix::Get_1int(int i) const{
+  if((i>numcols) || (i<1)){
+    cerr << "matrix index exceeded in Get" << endl;
+    exit(1);
+  }
+  return elements[i-1];
+}
+
+void RowMatrix::Set_1int(int i, double value){
+  if((i>numcols) || (i<1)){
+    cerr << "matrix index exceeded in Set" << endl;
+    exit(1);
+  }
+  elements[i-1] = value;
+}
+
+double RowMatrix::BasicGet_1int(int i) const{
+  return elements[i];
+}
+
+void RowMatrix::BasicSet_1int(int i, double value){
+  elements[i] = value;
+}
+
+void RowMatrix::BasicIncrement_1int(int i, double value){
+  elements[i] += value;
+}
+
+MatrixType RowMatrix::GetType() const{
+  return ROWMATRIX;
+}
+
+istream& RowMatrix::ReadData(istream& c){
+  int n;
+  c >> n;
+  Dim(n);
+  for(int i=0;i<n;i++)
+    c >> elements[i];
+
+  return c;
+}
+
+ostream& RowMatrix::WriteData(ostream& c) const{  //output
+  c << numcols << ' ';
+  for(int i=0;i<numcols;i++)
+    c << elements[i] << ' ';
+  return c;
+}
+
+void RowMatrix::AssignVM(const VirtualMatrix& A){
+  if( A.GetNumRows() != 1 ){
+    cerr << "error trying to write a 2D matrix to a collumn" << endl;
+    exit(1);
+  }
+  Dim( A.GetNumCols() );
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.BasicGet(0,i);
+}
+
+RowMatrix& RowMatrix::operator=(const RowMatrix& A){ // assignment operator
+  Dim(A.numcols);
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.elements[i];
+  return *this;
+}
+
+RowMatrix& RowMatrix::operator=(const VirtualRowMatrix& A){ // overloaded =
+  Dim( A.GetNumCols() );
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.BasicGet(i);
+  return *this;
+}
+
+RowMatrix& RowMatrix::operator=(const VirtualMatrix& A){ // overloaded =
+  if( A.GetNumRows() != 1 ){
+    cerr << "error trying to write a 2D matrix to a collumn" << endl;
+    exit(1);
+  }
+  Dim( A.GetNumCols() );
+  for(int i=0;i<numcols;i++)
+    elements[i] = A.BasicGet(0,i);
+  return *this;
+}
+
+RowMatrix& RowMatrix::operator*=(double b){
+  for(int i=0;i<numcols;i++)
+    elements[i] *= b;
+  return *this;
+}
+

lib/poems/rowmatrix.h

@@ -0,0 +1,53 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: rowmatrix.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef ROWMATRIX_H
+#define ROWMATRIX_H
+
+#include "virtualrowmatrix.h"
+
+class RowMatrix : public VirtualRowMatrix  {
+  double* elements;
+public:
+  RowMatrix();
+  ~RowMatrix();
+  RowMatrix(const RowMatrix& A);  // copy constructor
+  RowMatrix(const VirtualRowMatrix& A);  // copy constructor
+  RowMatrix(const VirtualMatrix& A);  // copy constructor
+  RowMatrix(int n);  // size constructor
+
+  void Const(double value);
+  double& operator_1int (int i); // array access
+  double Get_1int(int i) const;
+  void Set_1int(int i, double value);
+  double BasicGet_1int(int i) const;
+  void BasicSet_1int(int i, double value);
+  void BasicIncrement_1int(int i, double value);
+  void Dim(int n);
+  MatrixType GetType() const;
+  std::istream& ReadData(std::istream& c);
+  std::ostream& WriteData(std::ostream& c) const;
+
+  void AssignVM(const VirtualMatrix& A);
+  RowMatrix& operator=(const RowMatrix& A); // assignment operator
+  RowMatrix& operator=(const VirtualRowMatrix& A); // overloaded =
+  RowMatrix& operator=(const VirtualMatrix& A); // overloaded =
+  RowMatrix& operator*=(double b);
+};
+
+#endif

lib/poems/solver.cpp

@@ -0,0 +1,62 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: solver.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "solver.h"
+#include "system.h"
+#include "matrices.h"
+
+Solver::Solver(){
+
+}
+
+Solver::~Solver(){
+}
+
+void Solver::SetSystem(System* s){
+  system = s;
+  CreateModel();
+}
+
+void Solver::ComputeForces(){
+system->ComputeForces();
+}
+
+SolverType Solver::GetSolverType()
+{
+	return type;
+}
+
+Solver * Solver::GetSolver(SolverType solverToMake) //returning a pointer to a new Solver object of the appropriate type
+{
+	switch((int)solverToMake)
+	{
+		case ONSOLVER: return new OnSolver();
+		default: return NULL;
+	}
+}
+
+ColMatMap* Solver::GetState(){
+  return &state;
+}
+
+ColMatMap* Solver::GetStateDerivative(){
+  return &statedot;
+}
+
+ColMatMap* Solver::GetStateDerivativeDerivative(){
+  return &statedoubledot;
+}

lib/poems/solver.h

@@ -0,0 +1,59 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: solver.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:       anderk5@rpi.edu                                     *
+ *_________________________________________________________________________*/
+
+#ifndef SOLVER_H
+#define SOLVER_H
+#include <fstream>
+#include "colmatmap.h"
+#include "matrices.h"
+#include "defines.h"
+
+class System;
+
+class Solver{
+protected:
+  System* system;
+
+  
+  double time;
+  ColMatMap state;
+  ColMatMap statedot;
+  ColMatMap statedoubledot;  
+
+  SolverType type;
+
+virtual void ComputeForces();
+
+public:
+  Solver();
+  virtual ~Solver();
+  void SetSystem(System* s);
+  SolverType GetSolverType();
+  static Solver * GetSolver(SolverType solverToMake);
+
+  virtual void DeleteModel() = 0;
+  virtual void CreateModel() = 0;
+  virtual void Solve(double time, Matrix& FF) = 0;  
+  
+    
+    
+  ColMatMap* GetState();
+  ColMatMap* GetStateDerivative();
+  ColMatMap* GetStateDerivativeDerivative();
+};
+
+#endif

lib/poems/sphericaljoint.cpp

@@ -0,0 +1,264 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: sphericaljoint.cpp                                      *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+ 
+
+#include "sphericaljoint.h"
+#include "point.h"
+#include "matrixfun.h"
+#include "body.h"
+#include "fastmatrixops.h"
+#include "norm.h"
+#include "eulerparameters.h"
+#include "matrices.h"
+#include <iomanip>
+
+
+SphericalJoint::SphericalJoint(){
+  DimQandU(4,3);
+}
+SphericalJoint::~SphericalJoint(){
+}
+
+JointType SphericalJoint::GetType(){
+  return SPHERICALJOINT;
+}
+
+bool SphericalJoint::ReadInJointData(std::istream& in){
+  return true;
+}
+
+void SphericalJoint::WriteOutJointData(std::ostream& out){
+}
+
+Matrix SphericalJoint::GetForward_sP(){  
+  Mat3x3 sPa,sPl;
+  Matrix sP(6,3);
+  sPa.Identity();    
+  sPl.Zeros();
+  Vect3 temp = -(point2->position);  
+  
+  sPl(1,2) = temp(3);
+  sPl(1,3) = -temp(2);
+  
+  sPl(2,1) = -temp(3);
+  sPl(2,3) = temp(1);
+  
+  sPl(3,1) = temp(2);  
+  sPl(3,2) = -temp(1);
+         
+  sP=Stack(sPa,sPl);  
+  return sP;    
+}
+
+void SphericalJoint::UpdateForward_sP( Matrix& sP){
+    // sP is constant, do nothing.
+  // linear part is not constant
+}
+
+Matrix SphericalJoint::GetBackward_sP(){
+	cout<<" -----------"<<endl;
+	cout<<"Am I coming here "<<endl;
+	cout<<" -----------"<<endl;	
+  Mat3x3 sPa,sPl;
+  Matrix sP;
+  sPa.Identity();    
+  sPl.Zeros();          
+  sPl(3,2)=(point2->position(1));  
+  sPl(2,3)=-(point2->position(1));   
+  sP=Stack(sPa,sPl);
+  return sP;    
+}
+
+void SphericalJoint::UpdateBackward_sP( Matrix& sP){  
+  // sP is constant, do nothing.
+}
+
+void SphericalJoint::ComputeLocalTransform(){
+	Mat3x3 ko_C_k;
+	// Obtain the transformation matrix from euler parameters
+	EP_Transformation(q, ko_C_k);
+	FastMult(pk_C_ko,ko_C_k,pk_C_k);
+	}  
+
+
+void SphericalJoint::ForwardKinematics(){
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 pk_w_k;
+  
+  //cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
+  EP_Normalize(q);
+
+
+  // orientations
+  ComputeForwardTransforms();    
+
+  
+  //----------------------------------//
+  // COMPUTE POSITION VECTOR R12 aka GAMMA    
+  
+  FastNegMult(pk_C_k,point2->position,result1); // parents basis      
+  FastAdd(result1,point1->position,r12);      
+  
+  // compute position vector r21 
+  FastNegMult(k_C_pk,r12,r21); 
+
+  
+  
+  //----------------------------------//
+  // COMPUTE GLOBAL LOCATION  
+  FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);    
+  FastAdd(result1,body1->r,result1);  
+  FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);  
+  FastAdd(result1,result2,body2->r);  
+  
+  qdot_to_u(q, u, qdot);  
+  
+
+  //-----------------------------------  
+  // angular velocities  
+  
+  FastAssign(u,pk_w_k);
+  FastTMult(pk_C_k,body1->omega_k,result1);        
+  FastAdd(result1,pk_w_k,body2->omega_k);
+  FastMult(body2->n_C_k,body2->omega_k,body2->omega); 
+
+  
+  
+  //-----------------------------------  
+  
+  // compute velocities  
+  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);  
+  FastAdd(body1->v_k,result1,result2);
+  FastTMult(pk_C_k,result2,result1); // In body basis  
+  
+  FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);                 
+  FastAdd(result1,result2,body2->v_k);    // In body basis     
+  FastMult(body2->n_C_k,body2->v_k,body2->v); 
+  
+  
+  //------------------------------------------
+  //Compute the KE   
+  Matrix tempke;
+  tempke = T(body2->v)*(body2->v);    
+  double ke = 0.0;
+  ke = body2->mass*tempke(1,1);  
+  FastMult(body2->inertia,body2->omega_k,result1);
+  tempke= T(body2->omega_k)*result1;  
+  ke = 0.5*ke + 0.5*tempke(1,1);
+  body2->KE=ke;
+
+  //-----------------------------------  
+  // compute state explicit angular acceleration  // Has to be in body basis    
+  FastTMult(pk_C_k,body1->alpha_t,result2);  
+  FastCross(body2->omega_k,pk_w_k,result1);
+  FastAdd(result1,result2,body2->alpha_t); 
+   
+  //-----------------------------------  
+  // compute state explicit acceleration   
+  // NEED TO DO THIS COMPLETELY IN BODY BASIS 
+  
+  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
+  FastCross(body1->omega_k,result1,result2); 
+  FastTMult(pk_C_k,result2,result1); 
+  
+  //FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);  
+  FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);  
+  FastCross(body2->omega_k,result3,result2);    
+  FastAdd(result1,result2,result3); //wxwxr in body basis  
+  
+  FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4); 
+  FastTMult(pk_C_k,result4,result5);  
+  FastAssign(result5,result4);
+  
+  FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);   
+  FastAdd(result2,result4,result1); //alphaxr in body basis  
+  
+  FastTMult(pk_C_k,body1->a_t,result2);
+  FastTripleSum(result3,result1,result2,body2->a_t);     // in body basis  
+  
+  
+  //-----------------------------------    
+}
+
+// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
+void SphericalJoint::BackwardKinematics(){
+  cout<<"what about here "<<endl;
+  Vect3 result1,result2,result3,result4,result5;
+  Vect3 k_w_pk;
+
+  // orientations
+  ComputeBackwardTransforms();
+  
+
+  // compute position vector r21
+  //r21 = point2->position - k_C_pk * point1->position;
+  FastMult(k_C_pk,point1->position,result1);
+  FastSubt(point2->position,result1,r21);
+  
+
+  // compute position vector r21
+  FastNegMult(pk_C_k,r21,r12);
+
+  // compute global location
+  // body1->r = body2->r + body2->n_C_k * r21;
+  FastMult(body2->n_C_k,r21,result1);
+  FastAdd(body2->r,result1,body1->r);
+
+  // compute qdot (for revolute joint qdot = u)
+  // qdot = u
+  ColMatrix us(3);
+  /*us(1)=0;
+  us(2)=u(1);
+  us(3)=u(2);*/
+  EP_Derivatives(q,u,qdot);
+
+  // angular velocities
+ 
+  FastMult(body2->n_C_k,u,result2);
+  FastAdd(body2->omega,result2,body1->omega);
+  FastAssign(u,k_w_pk);
+  FastMult(pk_C_k,body2->omega_k,result1);
+  FastSubt(result1,k_w_pk,body1->omega_k);
+  cout<<"The program was here"<<endl;
+
+  // compute velocities
+  FastCross(body2->omega_k,r21,result1);
+  FastCross(point1->position,k_w_pk,result2);
+  FastAdd(body2->v_k,result1,result3);
+  FastMult(pk_C_k,result3,result4);
+  FastAdd(result2,result4,body1->v_k);
+  FastMult(body1->n_C_k,body1->v_k,body1->v);
+
+  // compute state explicit angular acceleration
+  FastCross(body1->omega_k,k_w_pk,result1);
+  FastMult(pk_C_k,body2->alpha_t,result2);
+  FastAdd(result1,result2,body1->alpha_t);
+  
+  // compute state explicit acceleration
+  FastCross(body2->alpha_t,point2->position,result1);
+  FastCross(body2->omega_k,point2->position,result2);
+  FastCross(body2->omega_k,result2,result3);
+  FastTripleSum(body2->a_t,result1,result3,result4);
+  FastMult(pk_C_k,result4,result5);
+
+  FastCross(point1->position,body1->alpha_t,result1);
+  FastCross(point1->position,body1->omega_k,result2);
+  FastCross(body1->omega_k,result2,result3);
+
+  FastTripleSum(result5,result1,result3,body1->a_t);
+  
+}

lib/poems/sphericaljoint.h

@@ -0,0 +1,44 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: sphericaljoint.h                                        *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef SPHERICALJOINT_H
+#define SPHERICALJOINT_H
+
+#include "joint.h"
+#include "vect3.h"
+#include "mat3x3.h"
+
+
+
+class SphericalJoint : public Joint  {
+  Matrix const_sP;
+public:
+  SphericalJoint();
+  ~SphericalJoint();
+  JointType GetType();
+  bool ReadInJointData(std::istream& in);
+  void WriteOutJointData(std::ostream& out);
+  Matrix GetForward_sP();
+  Matrix GetBackward_sP();
+  void UpdateForward_sP( Matrix& sP);
+  void UpdateBackward_sP( Matrix& sP);
+  void ComputeLocalTransform();
+  void ForwardKinematics();
+  void BackwardKinematics();
+};
+
+#endif

lib/poems/system.cpp

@@ -0,0 +1,596 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: system.cpp                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "system.h"
+#include "body.h"
+#include "joint.h"
+#include <math.h>
+
+
+System::System(){
+	mappings = NULL;
+}
+
+System::~System(){
+  Delete();
+}
+
+void System::Delete(){
+  delete [] mappings;
+  bodies.DeleteValues();
+  joints.DeleteValues();
+}
+
+int System::GetNumBodies(){
+  return bodies.GetNumElements();
+}
+
+int * System::GetMappings()
+{
+	return mappings;
+}
+
+void System::AddBody(Body* body){
+  bodies.Append(body);
+}
+
+void System::AddJoint(Joint* joint){
+  joints.Append(joint);
+}
+
+void System::SetTime(double t){
+  time = t;
+}
+
+double System::GetTime(){
+  return time;
+}
+
+void System::ComputeForces(){	
+	// NOT DOING ANYTHING AT THIS TIME
+  }  
+  
+bool System::ReadIn(istream& in){
+  int numbodies;
+  Body* body;
+  int bodytype;
+  char bodyname[256];
+  int index;  
+
+  // get number of bodies
+  in >> numbodies;
+
+  // bodies loop
+  for(int i=0;i<numbodies;i++){
+
+    // error check
+    in >> index;
+    if(index != i){
+      cerr << "Error reading bodies" << endl;
+      return false;
+    }
+
+    in >> bodytype >> bodyname;
+    body = NewBody(bodytype);
+
+    // type check
+    if(!body){
+      cerr << "Unrecognized body type '" << bodytype << "'" << endl;
+      return false;
+    }
+
+    // add the body
+    AddBody(body);
+
+    // set generic body info
+    body->ChangeName(bodyname);
+
+    // read in the rest of its data
+    if(!body->ReadIn(in)) return false;
+  }
+
+  // create a temporary array for fast indexed access
+  Body** bodyarray = bodies.CreateArray();
+
+  int numjoints;
+  int jointtype;
+  char jointname[256];
+  Joint* joint;
+  int body1, body2;
+  int point1, point2;
+
+  // get number of joints
+  in >> numjoints;
+
+  // joints loop
+  for(int i=0;i<numjoints;i++){
+
+    // error check
+    in >> index;
+    if(index != i){
+      cerr << "Error reading joints" << endl;
+      return false;
+    }
+
+    in >> jointtype >> jointname;
+    joint = NewJoint(jointtype);
+
+    // joint type check
+    if(!joint){
+      cerr << "Unrecognized joint type '" << jointtype << "'" << endl;
+      return false;
+    }
+
+    // add the joint
+    AddJoint(joint);
+
+    // set the generic joint info
+    joint->ChangeName(jointname);
+
+    in >> body1 >> body2;
+    if( !(body1<numbodies) || !(body2<numbodies) ){
+      cerr << "Body index out of range" << endl;
+      return false;
+    }
+    
+    joint->SetBodies(bodyarray[body1], bodyarray[body2]);
+
+    bodyarray[body1]->AddJoint(joint);
+    bodyarray[body2]->AddJoint(joint);
+    
+    in >> point1 >> point2;
+
+    joint->SetPoints(bodyarray[body1]->GetPoint(point1),bodyarray[body2]->GetPoint(point2));
+
+    // read in the rest of its data
+    if(!joint->ReadIn(in)){
+      return false;
+    }
+  }
+
+  // delete the temporary array
+  delete [] bodyarray;
+
+  return true;
+}
+
+void System::WriteOut(ostream& out){
+  // number of bodies
+  out << bodies.GetNumElements() << endl;
+
+  // bodies loop
+  int i = 0;
+  Body* body;
+  ListElement<Body>* b_ele = bodies.GetHeadElement();
+  while(b_ele !=0){
+    out << i << ' ';
+
+    body = b_ele->value;
+
+    // set the body ID for later identification
+    body->SetID(i);
+    
+    // write out the data
+    body->WriteOut(out);
+
+    i++; b_ele = b_ele->next;
+  }
+
+  // number of joints
+  out << joints.GetNumElements() << endl;  
+
+  // joints loop
+  i = 0;
+  Joint* joint;
+  ListElement<Joint>* j_ele = joints.GetHeadElement();
+  while(j_ele !=0){
+    out << i << ' ';
+    joint = j_ele->value;
+
+    // set the joint ID for later identification
+    joint->SetID(i);
+
+    // write out the data
+    joint->WriteOut(out);
+    
+    i++; j_ele = j_ele->next;
+  }
+}
+
+void System::ClearBodyIDs(){
+  ListElement<Body>* current = bodies.GetHeadElement();
+
+  while(current){
+    current->value->SetID(0);
+    current = current->next;
+  }
+}
+
+void System::ClearJointIDs(){
+  ListElement<Joint>* current = joints.GetHeadElement();
+
+  while(current){
+    current->value->SetID(0);
+    current = current->next;
+  }
+}
+
+
+void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space){
+
+ //-------------------------------------------------------------------------//    
+  // Declaring Temporary Entities 
+  //-------------------------------------------------------------------------//   
+     Body* body = NULL;
+     Body* prev;
+     Body* Inertial;
+     Point* origin;
+     Joint* joint;
+     Point* point_CM; 
+     Point* point_p;  
+     Point* point_k;  
+     Point* point_ch = NULL;
+     Vect3 r1,r2,r3,v1,v2,v3; 
+     Mat3x3 IM, N, PKCK,PKCN ;
+     ColMatrix qo, uo, q, qdot,w;
+     
+	 mappings = new int[nfree];
+	 for(int i = 0; i < nfree; i++)
+	 {
+		 mappings[i] = freelist[i];		 
+	 }
+     qo.Dim(4);
+     uo.Dim(3);
+     q.Dim(4);
+     qdot.Dim(4);     
+     PKCN.Identity();
+     PKCK.Identity();
+     w.Dim(3);
+	
+//-------------------------------------------------------------------------//    
+  // Setting up Inertial Frame, gravity and Origin  
+  //-------------------------------------------------------------------------//
+     Inertial= new InertialFrame;  
+     AddBody(Inertial);
+     
+     Vect3 temp1;  
+     temp1.Zeros();  
+     ((InertialFrame*) Inertial)->SetGravity(temp1);    
+     origin= new FixedPoint(temp1);
+     Inertial->AddPoint(origin);    
+//-------------------------------------------------------------------------//
+	double ** xh1 = new double*[nfree];
+	double ** xh2 = new double*[nfree];
+	
+	for (int i=0; i<nfree; i++){		
+ 		xh1[i] = new double[3];
+		xh2[i] = new double[3];
+	}	
+	for (int i=0; i<nfree; i++){		
+		for (int j=0; j<3; j++){
+			xh1[i][j] = xcm[mappings[i]-1][j];
+		}
+	}
+	
+//-------------------------------------------------------------------------//    
+// Begin looping over each body for recursive kinematics
+//-------------------------------------------------------------------------//
+    for(int i=0;i<nfree;i++){
+	    prev=Inertial;
+	    point_p=origin;               
+      
+     body = new RigidBody;   
+     body->mass=masstotal[mappings[i]-1];
+     IM(1,1)=inertia[mappings[i]-1][0];
+     IM(2,2)=inertia[mappings[i]-1][1];
+     IM(3,3)=inertia[mappings[i]-1][2];
+     IM(1,2)=0.0;
+     IM(1,3)=0.0;
+     IM(2,3)=0.0;
+     IM(2,1)=IM(1,2);
+     IM(3,1)=IM(1,3);
+     IM(3,2)=IM(2,3);
+     body->inertia = IM;   
+
+//-------------------------------------------------------//                  
+    
+    
+	    for (int k=0;k<3;k++){ 
+          r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];     
+			 r3(k+1) = xcm[mappings[i]-1][k];
+			 r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k]; 	  
+	    }
+	  
+     r2.Zeros(); 
+     
+     for (int k=1;k<=3;k++){     
+          N(k,1)=ex_space[mappings[i]-1][k-1];              
+          N(k,2)=ey_space[mappings[i]-1][k-1];              
+          N(k,3)=ez_space[mappings[i]-1][k-1];              
+         }                  
+     
+     PKCK=T(N);
+     PKCN=T(N);
+    
+     q.Zeros();     
+     EP_FromTransformation(q,N);
+                    
+     r1=PKCN*r1;
+     r3=PKCN*r3;
+     
+     for (int k=1;k<=3;k++){
+     w(k)=omega[mappings[i]-1][k-1];                                   
+     }     
+     
+     Vect3 cart_r, cart_v;
+     for (int k=1;k<=3;k++){
+	     cart_r(k)=xcm[mappings[i]-1][k-1];	     
+	     cart_v(k)=vcm[mappings[i]-1][k-1];
+	     }
+	     	     
+	     w=PKCN*w;     
+	     EP_Derivatives(q,w,qdot);
+	     
+     
+//-------------------------------------------------------------------------//    
+// Create bodies and joints with associated properties for POEMS
+//-------------------------------------------------------------------------//
+
+     point_CM = new FixedPoint(r2);
+     point_k = new FixedPoint(r1);
+     point_ch = new FixedPoint(r3);
+     body->AddPoint(point_CM);
+     body->AddPoint(point_k);
+     body->AddPoint(point_ch);
+     AddBody(body);    
+   
+     Mat3x3 One;
+     One.Identity();	
+	  ColMatrix qq=Stack(q,cart_r);
+          ColMatrix vv=Stack(qdot,cart_v);          
+          joint=new FreeBodyJoint;
+          AddJoint(joint);
+          joint->SetBodies(prev,body);
+          body->AddJoint(joint);
+          prev->AddJoint(joint);
+          joint->SetPoints(point_p,point_k);
+          joint->SetZeroOrientation(One);
+          joint->DimQandU(7,6);
+          joint->SetInitialState(qq,vv);
+          joint->ForwardKinematics();                             
+  }
+  for(int i = 0; i < nfree; i++) {
+	  delete [] xh1[i];
+	  delete [] xh2[i];
+  }
+  delete xh1;
+  delete xh2;  
+}
+
+
+void System::Create_System_LAMMPS(int numbodies, double *mass,double **inertia, double ** xcm, double ** xjoint,double **vcm,double **omega,double **ex_space, double **ey_space, double **ez_space, int b, int * mapping, int count){
+
+	//-------------------------------------------------------------------------//    
+  // Declaring Temporary Entities 
+	//-------------------------------------------------------------------------//   
+  	
+	Body* body = NULL;
+	Body* prev;
+	Body* Inertial;
+	Point* origin;
+	Joint* joint;
+	Point* point_CM;   
+	Point* point_p;    
+	Point* point_k;    
+	Point* point_ch = NULL;  
+	Vect3 r1,r2,r3,v1,v2,v3; 
+	Mat3x3 IM, N, PKCK,PKCN ;
+	ColMatrix qo, uo, q, qdot,w;
+	Vect3 cart_r, cart_v;
+	mappings = new int[b];
+	for(int i = 0; i < b; i++){
+		mappings[i] = mapping[i];		 
+	}	 
+	 
+     
+	qo.Dim(4);
+	uo.Dim(3);
+	q.Dim(4);
+	qdot.Dim(4);     
+	PKCN.Identity();
+	PKCK.Identity();
+	w.Dim(3);
+	
+	//-------------------------------------------------------------------------//    
+  // Setting up Inertial Frame, gravity and Origin  
+	//-------------------------------------------------------------------------//
+	Inertial= new InertialFrame;  
+	AddBody(Inertial);
+     
+	Vect3 temp1;  
+	temp1.Zeros();  
+	((InertialFrame*) Inertial)->SetGravity(temp1);    
+	origin= new FixedPoint(temp1);
+	Inertial->AddPoint(origin);    
+	//-------------------------------------------------------------------------//
+
+	double ** xh1;
+	double ** xh2;
+		  
+	xh1 = new double*[b];		  
+	xh2 = new double*[b];
+		  
+	
+	for (int i=0; i<b; i++){			
+		xh1[i] = new double[3];
+		xh2[i] = new double[3];			
+	}	
+		  
+		  
+		  
+	for (int j=0; j<3; j++){
+		xh1[0][j] = xcm[mapping[0]-1][j];
+		xh2[b-1][j] = xcm[mapping[b-1]-1][j];
+	}
+	
+	for (int i=0; i<b-1; i++){
+		for (int j=0; j<3; j++){
+			xh1[i+1][j] = xjoint[mapping[i]-count-1][j];
+		}
+	}		  
+	
+	for (int i=0; i<b-1; i++){
+		for (int j=0; j<3; j++){
+			xh2[i][j] = xjoint[mapping[i]-count-1][j];
+		}
+	}
+
+	
+	//-------------------------------------------------------------------------//    
+// Begin looping over each body for recursive kinematics
+	//-------------------------------------------------------------------------//
+	for(int i=0;i<b;i++){	    
+		if (i == 0){
+			prev=Inertial;
+			point_p=origin;
+		}    
+		else{
+			prev = body;            
+			point_p = point_ch;     
+		}   
+    
+  	  
+		body = new RigidBody;  
+		body->mass=mass[mapping[i]-1];
+		IM(1,1)=inertia[mapping[i]-1][0];
+		IM(2,2)=inertia[mapping[i]-1][1];
+		IM(3,3)=inertia[mapping[i]-1][2];
+		IM(1,2)=0.0;
+		IM(1,3)=0.0;
+		IM(2,3)=0.0;
+		IM(2,1)=IM(1,2);
+		IM(3,1)=IM(1,3);
+		IM(3,2)=IM(2,3);
+		body->inertia = IM;   
+     
+		//-------------------------------------------------------//
+             
+		for (int k=0;k<3;k++){
+			r1(k+1)=xh1[i][k]-xcm[mapping[i]-1][k]; 
+			r3(k+1)=xh2[i][k]-xcm[mapping[i]-1][k]; 
+		}
+		r2.Zeros();   
+     
+		for (int k=1;k<=3;k++){     
+			N(k,1)=ex_space[mapping[i]-1][k-1];              
+			N(k,2)=ey_space[mapping[i]-1][k-1];              
+			N(k,3)=ez_space[mapping[i]-1][k-1];              
+		}
+	 
+         
+		if (i==0){
+			PKCK=T(N);     
+			PKCN=T(N);     
+	     
+			q.Zeros();	     
+			EP_FromTransformation(q,N);
+	     
+			r1=PKCN*r1;
+			r3=PKCN*r3;
+	     
+			for (int k=1;k<=3;k++){     
+				w(k)=omega[mappings[i]-1][k-1];
+			}	     
+	     
+			for (int k=1;k<=3;k++){
+				cart_r(k)=xcm[mappings[i]-1][k-1];		     
+				cart_v(k)=vcm[mappings[i]-1][k-1];
+			}
+			w=PKCN*w;     
+			EP_Derivatives(q,w,qdot);    
+		 
+		}                 
+		else{	     
+			PKCK=PKCN*N;   
+			PKCN=T(N);     
+    
+			q.Zeros();
+			EP_FromTransformation(q,PKCK);
+               
+			r1=PKCN*r1;
+			r3=PKCN*r3;
+     
+			for (int k=1;k<=3;k++){     
+				w(k)=omega[mapping[i]-1][k-1]-omega[mapping[i-1]-1][k-1];                         
+			}
+    	
+			w=PKCN*w; 
+			EP_Derivatives(q, w, qdot);        
+		}  
+        
+	
+		//-------------------------------------------------------------------------//    
+// Create bodies and joints with associated properties for POEMS
+		//-------------------------------------------------------------------------//
+
+		point_CM = new FixedPoint(r2);
+		point_k = new FixedPoint(r1);
+		point_ch = new FixedPoint(r3);
+		body->AddPoint(point_CM);
+		body->AddPoint(point_k);
+		body->AddPoint(point_ch);
+		AddBody(body);    
+   
+		Mat3x3 One;
+		One.Identity();	
+		if (i==0){	  
+			ColMatrix qq=Stack(q,cart_r);
+			ColMatrix vv=Stack(qdot,cart_v);          
+			joint=new FreeBodyJoint;
+			AddJoint(joint);
+			joint->SetBodies(prev,body);
+			body->AddJoint(joint);
+			prev->AddJoint(joint);
+			joint->SetPoints(point_p,point_k);
+			joint->SetZeroOrientation(One);
+			joint->DimQandU(7,6);
+			joint->SetInitialState(qq,vv);  	  
+			joint->ForwardKinematics();  
+		}    
+		else{	     
+			joint= new SphericalJoint;
+			AddJoint(joint);
+			joint->SetBodies(prev,body);
+			body->AddJoint(joint);
+			prev->AddJoint(joint);
+			joint->SetPoints(point_p,point_k);          
+			joint->SetZeroOrientation(One);
+			joint->DimQandU(4,3);
+			joint->SetInitialState(q,qdot);
+			joint->ForwardKinematics();  
+		}
+	}
+	for(int i = 0; i < b; i++)
+	{
+		delete [] xh1[i];
+		delete [] xh2[i];
+	}
+	delete [] xh1;
+	delete [] xh2;
+  
+}

lib/poems/system.h

@@ -0,0 +1,92 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: system.h                                                *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+ 
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <stdio.h>   
+#include <iomanip>  
+
+#include "poemslist.h"
+#include "matrices.h"
+#include "workspace.h"
+#include "matrixfun.h"
+#include "onsolver.h"
+#include "system.h"
+#include "inertialframe.h"
+#include "rigidbody.h"
+#include "revolutejoint.h"
+#include "fixedpoint.h"
+#include "freebodyjoint.h"
+#include "sphericaljoint.h"
+#include "body23joint.h"
+#include "mixedjoint.h"
+#include "eulerparameters.h"
+#include "matrices.h"
+#include "norm.h"
+
+
+     class Body;
+     class Joint;
+
+     class System{
+	 private:
+		int * mappings;
+		
+     public:
+       double time;
+       List<Body> bodies;
+       List<Joint> joints;
+  
+       System();
+       ~System();
+       void Delete();
+       
+       int GetNumBodies();
+       
+	   int * GetMappings();
+
+       void AddBody(Body* body);
+       
+       void AddJoint(Joint* joint);
+       
+       void SetTime(double t);
+       
+       double GetTime();
+       
+       void ComputeForces();
+       
+       bool ReadIn(std::istream& in);
+       
+       void WriteOut(std::ostream& out);
+       
+       void ClearBodyIDs();
+       
+       void ClearJointIDs();              
+
+       void Create_System_LAMMPS(int numbodies, double *mass,double **inertia, double ** xcm, double ** xjoint,double **vh1,double **omega,double **ex_space, double **ey_space, double **ez_space, int b, int * mapping, int count);
+       
+       void Create_DegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space);
+
+};
+
+#endif

lib/poems/vect3.cpp

@@ -0,0 +1,148 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect3.cpp                                               *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "vect3.h"
+
+using namespace std;
+
+Vect3::Vect3(){
+  numrows = 3; numcols = 1;
+}
+Vect3::~Vect3(){
+}
+
+Vect3::Vect3(const Vect3& A){  // copy constructor
+  numrows = 3; numcols = 1;
+
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+}
+
+Vect3::Vect3(const VirtualMatrix& A){  // copy constructor
+  numrows = 3; numcols = 1;
+
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<3;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+double& Vect3::operator_1int (int i){ // array access
+	if(i<1 || i>3){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+  return elements[i-1];
+}
+
+double Vect3::Get_1int(int i) const{
+  return elements[i-1];
+}
+
+void Vect3::Set_1int(int i, double value){
+  elements[i-1] = value;
+}
+
+double Vect3::BasicGet_1int(int i) const{
+  return elements[i];
+}
+
+void Vect3::BasicSet_1int(int i, double value){
+  elements[i] = value;
+}
+
+void Vect3::BasicIncrement_1int(int i, double value){
+  elements[i] += value;
+}
+
+void Vect3::Const(double value){
+  elements[0] = value;
+  elements[1] = value;
+  elements[2] = value;
+}
+
+MatrixType Vect3::GetType() const{
+  return VECT3;
+}
+
+istream& Vect3::ReadData(istream& c){  //input
+  for(int i=0;i<3;i++)
+    c >> elements[i];
+  return c;
+}
+
+ostream& Vect3::WriteData(ostream& c) const{  //output
+  for(int i=0;i<3;i++)
+    c << elements[i] << ' ';
+  return c;
+}
+
+void Vect3::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+Vect3& Vect3::operator=(const Vect3& A){ // assignment operator
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+  return *this;
+}
+
+Vect3& Vect3::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 3) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+  return *this;
+}
+
+Vect3& Vect3::operator*=(double b){
+  elements[0] *= b;
+  elements[1] *= b;
+  elements[2] *= b;
+  return *this;
+}
+
+Vect3& Vect3::operator+=(const Vect3& A){
+  elements[0] += A.elements[0];
+  elements[1] += A.elements[1];
+  elements[2] += A.elements[2];
+  return *this;
+}
+
+Vect3& Vect3::operator-=(const Vect3& A){
+  elements[0] -= A.elements[0];
+  elements[1] -= A.elements[1];
+  elements[2] -= A.elements[2];
+  return *this;
+}

lib/poems/vect3.h

@@ -0,0 +1,84 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect3.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef VECT3_H
+#define VECT3_H
+
+#include "virtualcolmatrix.h"
+
+class Matrix;
+class Mat3x3;
+class Mat6x6;
+class Vect6;
+class ColMatrix;
+
+class Vect3 : public VirtualColMatrix  {
+  double elements[3];
+public:
+  Vect3();
+  ~Vect3();
+  Vect3(const Vect3& A);  // copy constructor
+  Vect3(const VirtualMatrix& A);  // copy constructor
+
+  double& operator_1int(int i); // array access
+  double Get_1int(int i) const;
+  void Set_1int(int i, double value);
+  double BasicGet_1int(int i) const;
+  void BasicSet_1int(int i, double value);
+  void BasicIncrement_1int(int i, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+  std::ostream& WriteData(std::ostream& c) const;
+  std::istream& ReadData(std::istream& c);
+
+  void AssignVM(const VirtualMatrix& A);
+  Vect3& operator=(const Vect3& A); // assignment operator
+  Vect3& operator=(const VirtualMatrix& A); // overloaded =
+  Vect3& operator*=(double b);
+  Vect3& operator+=(const Vect3& A);
+  Vect3& operator-=(const Vect3& A);
+
+  friend Matrix T(const Vect3& A);  // a wasteful transpose
+  friend Mat3x3 CrossMat(Vect3& a);  // a wasteful cross matrix implementation
+
+  friend void Set6DAngularVector(Vect6& v6, Vect3& v3);
+  friend void Set6DLinearVector(Vect6& v6, Vect3& v3);
+
+  // fast matrix functions
+  friend void FastAssign(Vect3& a, Vect3& c);
+  friend void FastSimpleRotation(Vect3& v, double q, Mat3x3& d);
+  friend void FastCross(Vect3& a, Vect3& b, Vect3& c); // cross product axb = c
+  friend void FastTripleSum(Vect3& a, Vect3& b, Vect3& c, Vect3& d);
+  friend void FastTripleSumPPM(Vect3& a, Vect3& b, Vect3& c, Vect3& d);
+  friend void FastMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastTMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C);
+  friend void FastMult(double a, Vect3& B, Vect3& C);
+  friend void FastAdd(Vect3& A, Vect3& B, Vect3& C);
+  friend void FastSubt(Vect3& A, Vect3& B, Vect3& C);
+  friend void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV);
+  friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
+
+  friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
+  friend void FastAssign(ColMatrix&A, Vect3& C);
+  friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);
+  
+};
+
+#endif

lib/poems/vect4.cpp

@@ -0,0 +1,135 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect4.cpp                                               *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "vect4.h"
+
+using namespace std;
+
+Vect4::Vect4(){
+  numrows = 4; numcols = 1;
+}
+Vect4::~Vect4(){
+}
+
+Vect4::Vect4(const Vect4& A){  // copy constructor
+  numrows = 4; numcols = 1;
+
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+  elements[3] = A.elements[3];
+}
+
+Vect4::Vect4(const VirtualMatrix& A){  // copy constructor
+  numrows = 4; numcols = 1;
+
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<4;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+double& Vect4::operator_1int (int i){ // array access
+  return elements[i-1];
+}
+
+double Vect4::Get_1int(int i) const{
+  return elements[i-1];
+}
+
+void Vect4::Set_1int(int i, double value){
+  elements[i-1] = value;
+}
+
+double Vect4::BasicGet_1int(int i) const{
+  return elements[i];
+}
+
+void Vect4::BasicSet_1int(int i, double value){
+  elements[i] = value;
+}
+
+void Vect4::BasicIncrement_1int(int i, double value){
+  elements[i] += value;
+}
+
+void Vect4::Const(double value){
+  elements[0] = value;
+  elements[1] = value;
+  elements[2] = value;
+  elements[3] = value;
+}
+
+MatrixType Vect4::GetType() const{
+  return VECT4;
+}
+
+istream& Vect4::ReadData(istream& c){  //input
+  for(int i=0;i<4;i++)
+    c >> elements[i];
+  return c;
+}
+
+ostream& Vect4::WriteData(ostream& c) const{  //output
+  for(int i=0;i<4;i++)
+    c << elements[i] << ' ';
+  return c;
+}
+
+void Vect4::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+Vect4& Vect4::operator=(const Vect4& A){ // assignment operator
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+  elements[3] = A.elements[3];
+  return *this;
+}
+
+Vect4& Vect4::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 4) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+  return *this;
+}
+
+Vect4& Vect4::operator*=(double b){
+  elements[0] *= b;
+  elements[1] *= b;
+  elements[2] *= b;
+  elements[3] *= b;
+  return *this;
+}
+

lib/poems/vect4.h

@@ -0,0 +1,71 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect4.h                                                 *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef VECT4_H
+#define VECT4_H
+
+#include "virtualcolmatrix.h"
+
+
+class Matrix;
+class Mat4x4;
+
+class Vect4 : public VirtualColMatrix  {
+  double elements[4];
+public:
+  Vect4();
+  ~Vect4();
+  Vect4(const Vect4& A);  // copy constructor
+  Vect4(const VirtualMatrix& A);  // copy constructor
+
+  double& operator_1int(int i); // array access
+  double Get_1int(int i) const;
+  void Set_1int(int i, double value);
+  double BasicGet_1int(int i) const;
+  void BasicSet_1int(int i, double value);
+  void BasicIncrement_1int(int i, double value);
+
+  
+  void Const(double value);
+  MatrixType GetType() const;
+  std::ostream& WriteData(std::ostream& c) const;
+  std::istream& ReadData(std::istream& c);
+
+  void AssignVM(const VirtualMatrix& A);
+  Vect4& operator=(const Vect4& A); // assignment operator
+  Vect4& operator=(const VirtualMatrix& A); // overloaded =
+  Vect4& operator*=(double b);
+
+  friend Matrix T(const Vect4& A);  // a wasteful transpose
+  friend Mat4x4 CrossMat(Vect4& a);  // a wasteful cross matrix implementation
+
+  // fast matrix functions
+  friend void FastAssign(Vect4& a, Vect4& c);
+  friend void FastSimpleRotation(Vect4& v, double q, Mat4x4& d);
+  friend void FastCross(Vect4& a, Vect4& b, Vect4& c); // cross product axb = c
+  friend void FastTripleSum(Vect4& a, Vect4& b, Vect4& c, Vect4& d);
+  friend void FastTripleSumPPM(Vect4& a, Vect4& b, Vect4& c, Vect4& d);
+  friend void FastMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastTMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C);
+  friend void FastMult(double a, Vect4& B, Vect4& C);
+  friend void FastAdd(Vect4& A, Vect4& B, Vect4& C);
+  friend void FastSubt(Vect4& A, Vect4& B, Vect4& C);
+};
+
+#endif

lib/poems/vect6.cpp

@@ -0,0 +1,147 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect6.cpp                                               *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "vect6.h"
+
+using namespace std;
+
+Vect6::Vect6(){
+  numrows = 6; numcols = 1;
+}
+Vect6::~Vect6(){
+}
+
+Vect6::Vect6(const Vect6& A){  // copy constructor
+  numrows = 6; numcols = 1;
+
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+  elements[3] = A.elements[3];
+  elements[4] = A.elements[4];
+  elements[5] = A.elements[5];
+}
+
+Vect6::Vect6(const VirtualMatrix& A){  // copy constructor
+  numrows = 6; numcols = 1;
+
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<6;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+double& Vect6::operator_1int (int i){ // array access
+	if(i<1 || i>6){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+	return elements[i-1];
+}
+
+double Vect6::Get_1int(int i) const{
+  return elements[i-1];
+}
+
+void Vect6::Set_1int(int i, double value){
+  elements[i-1] = value;
+}
+
+double Vect6::BasicGet_1int(int i) const{
+  return elements[i];
+}
+
+void Vect6::BasicSet_1int(int i, double value){
+  elements[i] = value;
+}
+
+void Vect6::BasicIncrement_1int(int i, double value){
+  elements[i] += value;
+}
+
+void Vect6::Const(double value){
+  elements[0] = value;
+  elements[1] = value;
+  elements[2] = value;
+  elements[3] = value;
+  elements[4] = value;
+  elements[5] = value;
+}
+
+MatrixType Vect6::GetType() const{
+  return VECT6;
+}
+
+istream& Vect6::ReadData(istream& c){  //input
+  for(int i=0;i<6;i++)
+    c >> elements[i];
+  return c;
+}
+
+ostream& Vect6::WriteData(ostream& c) const{  //output
+  for(int i=0;i<6;i++)
+    c << elements[i] << ' ';
+  return c;
+}
+
+void Vect6::AssignVM(const VirtualMatrix& A){
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+}
+
+Vect6& Vect6::operator=(const Vect6& A){ // assignment operator
+  elements[0] = A.elements[0];
+  elements[1] = A.elements[1];
+  elements[2] = A.elements[2];
+  elements[3] = A.elements[3];
+  elements[4] = A.elements[4];
+  elements[5] = A.elements[5];
+  return *this;
+}
+
+Vect6& Vect6::operator=(const VirtualMatrix& A){ // overloaded =
+  // error check
+  if( (A.GetNumRows() != 6) || (A.GetNumCols() != 1) ){
+    cerr << "illegal matrix size" << endl;
+    exit(0);
+  }
+
+  for(int i=0;i<numrows;i++)
+    elements[i] = A.BasicGet(i,0);
+  return *this;
+}
+
+Vect6& Vect6::operator*=(double b){
+  elements[0] *= b;
+  elements[1] *= b;
+  elements[2] *= b;
+  elements[3] *= b;
+  elements[4] *= b;
+  elements[5] *= b;
+  return *this;
+}
+

lib/poems/vect6.h

@@ -0,0 +1,70 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: vect6.h                                                 *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#ifndef VECT6_H
+#define VECT6_H
+
+#include "virtualcolmatrix.h"
+
+class Matrix;
+class Mat6x6;
+class ColMatrix;
+class Vect3;
+
+class Vect6 : public VirtualColMatrix  {
+  double elements[6];
+public:
+  Vect6();
+  ~Vect6();
+  Vect6(const Vect6& A);  // copy constructor
+  Vect6(const VirtualMatrix& A);  // copy constructor
+
+  double& operator_1int (int i); // array access
+  double Get_1int(int i) const;
+  void Set_1int(int i, double value);
+  double BasicGet_1int(int i) const;
+  void BasicSet_1int(int i, double value);
+  void BasicIncrement_1int(int i, double value);
+
+  void Const(double value);
+  MatrixType GetType() const;
+  std::ostream& WriteData(std::ostream& c) const;
+  std::istream& ReadData(std::istream& c);
+
+  void AssignVM(const VirtualMatrix& A);
+  Vect6& operator=(const Vect6& A); // assignment operator
+  Vect6& operator=(const VirtualMatrix& A); // overloaded =
+  Vect6& operator*=(double b);
+
+  friend Matrix T(const Vect6& A);  // a wasteful transpose
+
+  friend void Set6DAngularVector(Vect6& v6, Vect3& v3);
+  friend void Set6DLinearVector(Vect6& v6, Vect3& v3);
+
+  // fast matrix operations
+  friend void FastAdd(Vect6& A, Vect6& B, Vect6& C);
+  friend void FastSubt(Vect6& A, Vect6& B, Vect6& C);
+  friend void FastMult(Mat6x6& A, Vect6& B, Vect6& C);
+  friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C);
+  friend void FastTMult(Mat6x6& A, Vect6& B, Vect6& C);
+  friend void FastTMult(Matrix& A, Vect6& B, ColMatrix& C);
+  friend void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C);
+
+  friend void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV);
+};
+
+#endif

lib/poems/virtualcolmatrix.cpp

@@ -0,0 +1,65 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualcolmatrix.cpp                                    *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+#include "virtualcolmatrix.h"
+#include <iostream>
+
+using namespace std;
+
+VirtualColMatrix::VirtualColMatrix(){
+  numcols = 1;
+}
+
+VirtualColMatrix::~VirtualColMatrix(){
+}
+
+double& VirtualColMatrix::operator_2int(int i, int j){
+	if(j!=1){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+	return (*this).operator_1int(i);
+}
+
+double VirtualColMatrix::Get_2int(int i, int j) const{
+  if(j!=1){
+    cerr << "Subscript out of bounds for collumn matrix" << endl;
+    exit(1);
+  }
+  return Get_1int(i);
+}
+
+void VirtualColMatrix::Set_2int(int i, int j, double value){
+  if(j!=1){
+    cerr << "Subscript out of bounds for collumn matrix" << endl;
+    exit(1);
+  }
+  Set_1int(i,value);
+}
+
+double VirtualColMatrix::BasicGet_2int(int i, int j) const{
+  return BasicGet_1int(i);
+}
+
+void VirtualColMatrix::BasicSet_2int(int i, int j, double value){
+  BasicSet_1int(i,value);
+}
+
+void VirtualColMatrix::BasicIncrement_2int(int i, int j, double value){
+  BasicIncrement_1int(i,value);
+}
+

lib/poems/virtualcolmatrix.h

@@ -0,0 +1,45 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualcolmatrix.h                                      *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef VIRTUALCOLMATRIX_H
+#define VIRTUALCOLMATRIX_H
+
+#include "virtualmatrix.h"
+
+
+class VirtualColMatrix : public VirtualMatrix  {
+public: 
+	VirtualColMatrix();
+	~VirtualColMatrix();
+	double& operator_2int (int i, int j); // array access
+	double Get_2int (int i, int j) const;
+	void Set_2int (int i, int j, double value);
+	double BasicGet_2int(int i, int j) const;
+	void BasicSet_2int(int i, int j, double value);
+	void BasicIncrement_2int(int i, int j, double value);
+
+	virtual double& operator_1int (int i) = 0; // array access
+	virtual double Get_1int(int i) const = 0;
+	virtual void Set_1int(int i, double value) = 0;
+	virtual double BasicGet_1int(int i) const = 0;
+	virtual void BasicSet_1int(int i, double value) = 0;
+	virtual void BasicIncrement_1int(int i, double value) = 0;
+
+};
+
+#endif

lib/poems/virtualmatrix.cpp

@@ -0,0 +1,170 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualmatrix.cpp                                       *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "virtualmatrix.h"
+#include "matrixfun.h"
+
+using namespace std;
+
+VirtualMatrix::VirtualMatrix(){
+  numrows = numcols = 0;
+}
+
+VirtualMatrix::~VirtualMatrix(){
+}
+
+int VirtualMatrix::GetNumRows() const {
+  return numrows;
+}
+
+int VirtualMatrix::GetNumCols() const {
+  return numcols;
+}
+
+double& VirtualMatrix::operator() (int i, int j){ // array access
+	return operator_2int(i,j);
+}
+
+double VirtualMatrix::Get(int i, int j) const{
+	return Get_2int(i,j);
+}
+
+void VirtualMatrix::Set(int i, int j, double value){
+	Set_2int(i,j,value);
+}
+
+double VirtualMatrix::BasicGet(int i, int j) const{
+	return BasicGet_2int(i,j);
+}
+
+void VirtualMatrix::BasicSet(int i, int j, double value){
+	BasicSet_2int(i,j,value);
+}
+
+void VirtualMatrix::BasicIncrement(int i, int j, double value){
+	BasicIncrement_2int(i,j,value);
+}
+
+double& VirtualMatrix::operator() (int i){
+	return operator_1int(i);
+}
+
+double VirtualMatrix::Get(int i) const{
+	return Get_1int(i);
+}
+
+void VirtualMatrix::Set(int i, double value){
+	Set_1int(i, value);
+}
+
+double VirtualMatrix::BasicGet(int i) const{
+	return BasicGet_1int(i);
+}
+
+void VirtualMatrix::BasicSet(int i, double value){
+	BasicSet_1int(i, value);
+}
+
+void VirtualMatrix::BasicIncrement(int i, double value){
+	BasicIncrement_1int(i, value);
+}
+
+double& VirtualMatrix::operator_1int (int i) {
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+	return *(new double);
+}
+
+double VirtualMatrix::Get_1int(int i) const {
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+	return 0.0;
+}
+
+void VirtualMatrix::Set_1int(int i, double value){
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+}
+
+double VirtualMatrix::BasicGet_1int(int i) const {
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+	return 0.0;
+}
+
+void VirtualMatrix::BasicSet_1int(int i, double value) {
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+}
+
+void VirtualMatrix::BasicIncrement_1int(int i, double value){
+	cerr << "Error: single dimensional access is not defined for matrices of type " << GetType() << endl;
+	exit(0);
+}
+
+void VirtualMatrix::Zeros(){
+  Const(0.0);
+}
+
+void VirtualMatrix::Ones(){
+  Const(1.0);
+}
+
+ostream& VirtualMatrix::WriteData(ostream& c) const {
+  cerr << "Error: no output definition for matrices of type " << GetType() << endl;
+  exit(0);
+}
+
+istream& VirtualMatrix::ReadData(istream& c){
+	cerr << "Error: no input definition for matrices of type " << GetType() << endl;
+	exit(0);
+}
+
+//
+// operators and functions
+//
+
+ostream& operator<< (ostream& c, const VirtualMatrix& A){  //output
+  c << A.GetType() << ' ';
+  A.WriteData(c);
+  c << endl;
+  return c;
+}
+
+istream& operator>> (istream& c, VirtualMatrix& A){  //input
+  VirtualMatrix* vm;
+  int matrixtype;
+  c >> matrixtype;
+
+  if( MatrixType(matrixtype) == A.GetType() ) A.ReadData(c);
+  else{
+    // issue a warning?
+    cerr << "Warning: During matrix read expected type " << A.GetType() << " and got type " << matrixtype << endl;
+    vm = NewMatrix(matrixtype);
+    if(!vm){
+      cerr << "Error: unable to instantiate matrix of type " << matrixtype << endl;
+      exit(0);
+    }
+    vm->ReadData(c);
+    A.AssignVM(*vm);
+    delete vm;
+  }
+
+  return c;
+}
+

lib/poems/virtualmatrix.h

@@ -0,0 +1,87 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualmatrix.h                                              *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef VIRTUALMATRIX_H
+#define VIRTUALMATRIX_H
+#include <iostream>
+
+enum MatrixType {
+	MATRIX = 0,
+	COLMATRIX = 1,
+	ROWMATRIX = 2,
+	MAT3X3 = 3,
+	VECT3 = 4,
+	MAT6X6 = 5,
+	VECT6 = 6,
+	COLMATMAP = 7,
+	VECT4 = 8,
+	MAT4X4 = 9
+};
+
+class VirtualMatrix {
+protected:
+	int numrows, numcols;
+public:
+	VirtualMatrix();
+	virtual ~VirtualMatrix();
+	int GetNumRows() const;
+	int GetNumCols() const;
+
+	double& operator() (int i, int j); // array access
+	double Get(int i, int j) const;
+	void Set(int i, int j, double value);
+	double BasicGet(int i, int j) const;
+	void BasicSet(int i, int j, double value);
+	void BasicIncrement(int i, int j, double value);
+
+	double& operator() (int i); // array access
+	double Get(int i) const;
+	void Set(int i, double value);
+	double BasicGet(int i) const;
+	void BasicSet(int i, double value);
+	void BasicIncrement(int i, double value);
+
+	virtual void Const(double value) = 0;
+	virtual MatrixType GetType() const = 0;
+	virtual void AssignVM(const VirtualMatrix& A) = 0;
+	void Zeros();
+	void Ones();
+	virtual std::ostream& WriteData(std::ostream& c) const;
+	virtual std::istream& ReadData(std::istream& c);
+
+protected:
+	virtual double& operator_2int(int i, int j) = 0;
+	virtual double& operator_1int(int i);
+	virtual double Get_2int(int i, int j) const = 0;
+	virtual double Get_1int(int i) const ;
+	virtual void Set_2int(int i, int j, double value) = 0;
+	virtual void Set_1int(int i, double value);
+	virtual double BasicGet_2int(int i, int j) const = 0;
+	virtual double BasicGet_1int(int i) const ;
+	virtual void BasicSet_2int(int i, int j, double value) = 0;
+	virtual void BasicSet_1int(int i, double value);
+	virtual void BasicIncrement_2int(int i, int j, double value) = 0;
+	virtual void BasicIncrement_1int(int i, double value);
+
+};
+
+// overloaded operators
+std::ostream& operator<< (std::ostream& c, const VirtualMatrix& A); // output
+std::istream& operator>> (std::istream& c, VirtualMatrix& A); // input
+
+#endif

lib/poems/virtualrowmatrix.cpp

@@ -0,0 +1,67 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualrowmatrix.cpp                                    *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "virtualrowmatrix.h"
+#include <iostream>
+
+using namespace std;
+
+VirtualRowMatrix::VirtualRowMatrix(){
+  numrows = 1;
+}
+
+VirtualRowMatrix::~VirtualRowMatrix(){
+}
+
+double& VirtualRowMatrix::operator_2int (int i, int j){
+	if(i!=1){
+		cerr << "matrix index invalid in operator ()" << endl;
+		exit(1);
+	}
+  return (*this).operator_1int(j);
+}
+
+double VirtualRowMatrix::Get_2int(int i, int j) const{
+  if(i!=1){
+    cerr << "Subscript out of bounds for row matrix" << endl;
+    exit(1);
+  }
+  return Get_1int(j);
+}
+
+void VirtualRowMatrix::Set_2int(int i, int j, double value){
+  if(i!=1){
+    cerr << "Subscript out of bounds for row matrix" << endl;
+    exit(1);
+  }
+  Set_1int(j,value);
+}
+
+double VirtualRowMatrix::BasicGet_2int(int i, int j) const{
+  return BasicGet_1int(j);
+}
+
+void VirtualRowMatrix::BasicSet_2int(int i, int j, double value){
+  BasicSet_1int(j,value);
+}
+
+void VirtualRowMatrix::BasicIncrement_2int(int i, int j, double value){
+  BasicIncrement_1int(j,value);
+}
+
+

lib/poems/virtualrowmatrix.h

@@ -0,0 +1,43 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: virtualrowmatrix.h                                      *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef VIRTUALROWMATRIX_H
+#define VIRTUALROWMATRIX_H
+
+#include "virtualmatrix.h"
+
+class VirtualRowMatrix : public VirtualMatrix  {
+public: 
+  VirtualRowMatrix();
+  ~VirtualRowMatrix();
+  double& operator_2int (int i, int j); // array access
+  double Get_2int(int i, int j) const;
+  void Set_2int(int i, int j, double value);
+  double BasicGet_2int(int i, int j) const;
+  void BasicSet_2int(int i, int j, double value);
+  void BasicIncrement_2int(int i, int j, double value);
+  
+  virtual double& operator_1int (int i) = 0; // array access
+  virtual double Get_1int(int i) const = 0;
+  virtual void Set_1int(int i, double value) = 0;
+  virtual double BasicGet_1int(int i) const = 0;
+  virtual void BasicSet_1int(int i, double value) = 0;
+  virtual void BasicIncrement_1int(int i, double value) = 0;
+};
+
+#endif

lib/poems/workspace.cpp

@@ -0,0 +1,489 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: workspace.cpp                                           *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#include "workspace.h"
+#include "system.h"
+#include "solver.h"
+#include "SystemProcessor.h"
+#include <iostream>
+#include <fstream>
+#include <iomanip>
+#include <cmath>
+
+
+using namespace std;
+
+void Workspace::allocateNewSystem() {
+  currentIndex++;
+  if(currentIndex < maxAlloc)
+  {
+	  system[currentIndex].system = new System;
+  }
+  else
+  {
+	  maxAlloc = (maxAlloc + 1) * 2; 
+									 
+	  SysData * tempPtrSys = new SysData[maxAlloc];
+	  for(int i = 0; i < currentIndex; i++)
+	  {
+		  tempPtrSys[i] = system[i];
+	  }
+	  delete [] system;				  
+	  system = tempPtrSys;
+	  system[currentIndex].system = new System; 
+  }
+}
+
+Workspace::Workspace(){
+	currentIndex = -1; 
+	maxAlloc = 0;		
+	system = NULL;		
+}
+
+Workspace::~Workspace(){
+  for(int i = 0; i <= currentIndex; i++){
+	  delete system[i].system;
+  }
+  delete [] system;						
+}
+
+
+bool Workspace::LoadFile(char* filename){
+  bool ans;
+  ifstream file;
+  file.open(filename, ifstream::in);
+  if( !file.is_open() ){
+    cerr << "File '" << filename << "' not found." << endl;
+    return false;
+  }
+  allocateNewSystem();
+  ans = system[currentIndex].system->ReadIn(file);
+  file.close();
+
+  return ans;
+}
+
+void Workspace::SetLammpsValues(double dtv, double dthalf, double tempcon){
+	Thalf = dthalf;
+	Tfull = dtv;
+	ConFac = tempcon;	
+}
+
+
+bool Workspace::MakeSystem(int& nbody, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space, int &njoint, int **&jointbody, double **&xjoint, int& nfree, int*freelist, double dthalf, double dtv, double tempcon, double KE){
+	
+	SetLammpsValues(dtv, dthalf, tempcon);		
+	
+if(njoint){		
+	SystemProcessor sys;	
+	sys.processArray(jointbody, njoint);  	
+	List<POEMSChain> * results = sys.getSystemData();		
+
+	int numsyschains = results->GetNumElements();	
+	int headvalue = 0;
+	List<POEMSChain> * newresults = results;	
+	ListElement<POEMSChain> * tempNode = results->GetHeadElement();			
+	int stop = 1;
+	int counter = 1;
+	for(int n = 1; n<=numsyschains; n++){						
+		while(stop){			
+			if ( (*(tempNode->value->listOfNodes.GetHeadElement()->value) == (headvalue+1) ) || (*(tempNode->value->listOfNodes.GetTailElement()->value) == (headvalue+1) ) ) {			
+			newresults->Append(tempNode->value);								
+			headvalue = headvalue + tempNode->value->listOfNodes.GetNumElements();						
+			tempNode = results->GetHeadElement();					
+			stop = 0;			
+			counter ++;						
+		}		
+		else{						
+			tempNode = tempNode->next;						
+		}				
+		}
+		stop=1;			
+	}		
+	
+	ListElement<POEMSChain> * TNode = newresults->GetHeadElement();
+	ListElement<POEMSChain> * TTNode = newresults->GetHeadElement();
+	for(int kk=1; kk<=numsyschains; kk++){		
+		if(kk!=numsyschains)
+			TTNode = TNode->next;
+		newresults->Remove(TNode);			
+		if(kk!=numsyschains)
+			TNode = TTNode;		
+	}
+	ListElement<POEMSChain> * NodeValue = newresults->GetHeadElement();
+	int count = 0; 	
+	int * array;
+	int ** arrayFromChain;
+	int numElementsInSystem;
+	int ttk = 0;
+	
+		
+	while(NodeValue != NULL) {
+		array = new int[NodeValue->value->listOfNodes.GetNumElements()];
+		arrayFromChain = NodeValue->value->listOfNodes.CreateArray();
+		numElementsInSystem = NodeValue->value->listOfNodes.GetNumElements();								
+		for(int counter = 0; counter < numElementsInSystem; counter++){
+			array[counter] = *arrayFromChain[counter];
+		}
+				
+		SetKE(1,KE);
+		allocateNewSystem();			
+		system[currentIndex].system->Create_System_LAMMPS(nbody,masstotal,inertia,xcm,xjoint,vcm,omega,ex_space,ey_space,ez_space, numElementsInSystem, array, count);					
+		
+		system[currentIndex].solver = ONSOLVER;
+		ttk = ttk + 1;		
+		count = ttk;
+		delete [] array;
+		delete [] arrayFromChain;
+		NodeValue= NodeValue->next;						
+	}
+}
+if(nfree){
+	MakeDegenerateSystem(nfree,freelist,masstotal,inertia,xcm,vcm,omega,ex_space,ey_space,ez_space);
+}
+	return true;
+}
+
+
+bool Workspace::MakeDegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space){
+	allocateNewSystem();
+	system[currentIndex].system->Create_DegenerateSystem(nfree,freelist,masstotal,inertia,xcm,vcm,omega,ex_space,ey_space,ez_space);
+	system[currentIndex].solver = ONSOLVER;
+	return true;			
+} 
+
+bool Workspace::SaveFile(char* filename, int index){ 
+  if(index < 0){
+	  index = currentIndex; 
+  }
+  ofstream file;
+
+  file.open(filename, ofstream::out);
+  
+  if( !file.is_open() ){
+    cerr << "File '" << filename << "' could not be opened." << endl;
+    return false;
+  }
+  if(index >= 0 && index <= currentIndex){
+	  system[index].system->WriteOut(file);
+  }
+  else {
+	  cerr << "Error, requested system index " << index << ", minimum index 0 and maximum index " << currentIndex << endl;
+  }
+  file.close();
+  return true;
+}
+
+
+System* Workspace::GetSystem(int index){
+	if(index <= currentIndex){
+		if(index >= 0){
+			return system[index].system;			
+		}
+		else{
+			return system[currentIndex].system; 
+		}
+	}
+	else{
+		return NULL;
+	}
+}
+
+void Workspace::AddSolver(Solver* s, int index){
+	if(currentIndex >= index){
+		if(index >= 0){
+			system[index].solver = (int)s->GetSolverType();
+		}
+		else{
+			system[currentIndex].solver = (int)s->GetSolverType();
+		}
+	}
+	else{
+		cout << "Error adding solver to index " << index << endl;
+	}
+}
+
+int Workspace::getNumberOfSystems(){
+	return currentIndex + 1;
+}
+
+
+
+void Workspace::SetKE(int temp, double SysKE){	
+KE_val = SysKE;
+FirstTime =temp;
+}
+    
+
+void Workspace::LobattoOne(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
+	
+	int numsys = currentIndex;
+	int numbodies;
+	double time = 0.0;
+	int * mappings;
+	double SysKE=0.0;
+		
+	for (int i = 0; i <= numsys; i++){
+		mappings = system[i].system->GetMappings();
+		numbodies = system[i].system->GetNumBodies() - 1;
+		Matrix FF(6,numbodies);
+		FF.Zeros();
+		for (int j=1; j<=numbodies; j++){
+			FF(1,j)  = torque[mappings[j - 1]-1][0]*ConFac;
+			FF(2,j)  = torque[mappings[j - 1]-1][1]*ConFac;
+			FF(3,j)  = torque[mappings[j - 1]-1][2]*ConFac;
+			
+			FF(4,j)  = fcm[mappings[j - 1]-1][0]*ConFac;
+			FF(5,j)  = fcm[mappings[j - 1]-1][1]*ConFac;
+			FF(6,j)  = fcm[mappings[j - 1]-1][2]*ConFac;
+		}
+		
+		//------------------------------------//
+		// Get a solver and solve that system.
+		Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);				
+		theSolver->SetSystem(system[i].system);	
+		theSolver->Solve(time, FF);	
+		
+		
+		theSolver->Solve(time, FF);		
+		ColMatrix tempx = *((*theSolver).GetState());
+		ColMatrix tempv = *((*theSolver).GetStateDerivative());
+		ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
+		
+		
+		for(int numint =0 ; numint<3; numint++){
+			theSolver->Solve(time, FF);
+			tempa = *((*theSolver).GetStateDerivativeDerivative());
+			*((*theSolver).GetStateDerivative())= tempv + Thalf*tempa;
+		}
+		
+		ColMatrix TempV= *((*theSolver).GetStateDerivative());
+		*((*theSolver).GetState())= tempx + Tfull*TempV;
+				
+		int numjoints = system[i].system->joints.GetNumElements();
+		for(int k = 0; k < numjoints; k++){
+			system[i].system->joints(k)->ForwardKinematics();
+		}		
+		
+		for(int k = 0; k < numbodies; k++){
+			Vect3 temp1 =system[i].system->bodies(k+1)->r;
+			Vect3 temp2 =system[i].system->bodies(k+1)->v;
+			Vect3 temp3 =system[i].system->bodies(k+1)->omega;
+			Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;			
+			for(int m = 0; m < 3; m++){
+				xcm[mappings[k]-1][m]   = temp1(m+1);
+				vcm[mappings[k]-1][m]   = temp2(m+1);
+				omega[mappings[k]-1][m] = temp3(m+1);				
+				ex_space[mappings[k]-1][m] = temp4(m+1,1);				
+				ey_space[mappings[k]-1][m] = temp4(m+1,2);
+				ez_space[mappings[k]-1][m] = temp4(m+1,3);				
+			}			
+			SysKE = SysKE + system[i].system->bodies(k+1)->KE;
+		} 		
+		delete theSolver;		      
+	}	
+}
+
+void Workspace::LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm){
+	int numsys = currentIndex;
+	int numbodies;				
+	double time = 0.0;   
+	int * mappings;
+	double SysKE =0.0;		
+	for (int i = 0; i <= numsys; i++){
+		mappings = system[i].system->GetMappings();
+		numbodies = system[i].system->GetNumBodies() - 1;
+		Matrix FF(6,numbodies);				
+				
+		for (int j=1; j<=numbodies; j++){
+			FF(1,j)  = torque[mappings[j - 1]-1][0]*ConFac;
+			FF(2,j)  = torque[mappings[j - 1]-1][1]*ConFac;
+			FF(3,j)  = torque[mappings[j - 1]-1][2]*ConFac;
+			FF(4,j)  = fcm[mappings[j - 1]-1][0]*ConFac;
+			FF(5,j)  = fcm[mappings[j - 1]-1][1]*ConFac;
+			FF(6,j)  = fcm[mappings[j - 1]-1][2]*ConFac;
+		}			
+		
+		//------------------------------------//
+		// Get a solver and solve that system.		
+		Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);		
+		theSolver->SetSystem(system[i].system);    			
+		theSolver->Solve(time, FF);		
+		ColMatrix tempv = *((*theSolver).GetStateDerivative());
+		ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
+		*((*theSolver).GetStateDerivative()) = tempv + Thalf*tempa;
+		
+						
+		int numjoints = system[i].system->joints.GetNumElements();
+		for(int k = 0; k < numjoints; k++){
+			system[i].system->joints(k)->ForwardKinematics(); 
+		}				
+		    		
+		for(int k = 0; k < numbodies; k++){
+			Vect3 temp1 =system[i].system->bodies(k+1)->r;			
+			Vect3 temp2 =system[i].system->bodies(k+1)->v;	    	
+			Vect3 temp3 =system[i].system->bodies(k+1)->omega;
+			SysKE = SysKE + system[i].system->bodies(k+1)->KE;
+			for(int m = 0; m < 3; m++){
+				vcm[mappings[k]-1][m]   = temp2(m+1);
+				omega[mappings[k]-1][m] = temp3(m+1);
+			}			
+		}
+		delete theSolver;
+	}	
+}
+
+    
+ 
+  void Workspace::RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
+	
+	  double a[6];
+	  double b[6][6];
+	  double c[6];
+	  //double e[6];
+	 
+	  a[1] = 0.2;
+	  a[2] = 0.3;
+	  a[3] = 0.6;
+	  a[4] = 1.0;
+	  a[5] = 0.875;
+
+	  b[1][0] = 0.2;
+	  b[2][0] = 0.075;           b[2][1] = 0.225;
+	  b[3][0] = 0.3;             b[3][1] = -0.9;        b[3][2] = 1.2;
+	  b[4][0] = -11.0/54.0;      b[4][1] = 2.5;         b[4][2] = -70.0/27.0;    b[4][3] = 35.0/27.0;
+	  b[5][0] = 1631.0/55296.0; b[5][1] = 175.0/512.0; b[5][2] = 575.0/13824.0; b[5][3] = 44275.0/110592.0; b[5][4] = 253.0/4096.0;
+
+	  c[0] = 37.0/378.0;
+	  c[1] = 0.0;
+	  c[2] = 250.0/621.0;
+	  c[3] = 125.0/594.0;
+	  c[4] = 0.0;
+	  c[5] = 512.0/1771.0;
+
+	  int numsys = currentIndex; 
+	  int numbodies;				
+	  double time = 0.0;   
+	  int * mappings;
+	  double SysKE =0.0;
+
+	  
+	  for (int i = 0; i <= numsys; i++){
+		  mappings = system[i].system->GetMappings();  
+
+		  numbodies = system[i].system->GetNumBodies() - 1;
+		  Matrix FF(6,numbodies);
+		  for (int j=1; j<=numbodies; j++){
+			  FF(1,j)  = ConFac*torque[mappings[j - 1]-1][0];
+			  FF(2,j)  = ConFac*torque[mappings[j - 1]-1][1];
+			  FF(3,j)  = ConFac*torque[mappings[j - 1]-1][2];            			   
+
+			  FF(4,j)  = ConFac*fcm[mappings[j - 1]-1][0];
+			  FF(5,j)  = ConFac*fcm[mappings[j - 1]-1][1];
+			  FF(6,j)  = ConFac*fcm[mappings[j - 1]-1][2];
+		  }					  
+		
+		
+		  //------------------------------------//
+		// Get a solver and solve that system.		
+		  Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);		
+		  theSolver->SetSystem(system[i].system);    			
+		  theSolver->Solve(time, FF);	
+	
+		  ColMatrix initial_x;  
+		  ColMatrix initial_xdot;
+		  ColMatMap* x;
+		  ColMatMap* xdot;
+		  ColMatMap* xdotdot;
+	
+		  x = theSolver->GetState();  
+		  xdot = theSolver->GetStateDerivative();
+		  xdotdot=theSolver->GetStateDerivativeDerivative(); 
+	
+		  initial_x = *x;  
+		  initial_xdot = *xdot;  
+		  ColMatrix f[6];
+		  ColMatrix ff[6];
+	
+		  ff[0] = initial_xdot; 
+		  f[0] = *xdotdot; 
+	
+		  for(int ii=1;ii<6;ii++){
+			  time = a[ii] * Tfull;		
+			  (*x) = initial_x;		
+			  (*xdot) = initial_xdot;		
+			  for(int j=0;j<ii;j++){
+				  (*x) = (*x) + (b[ii][j]*Tfull)*ff[j];      		
+				  (*xdot) = (*xdot) + (b[ii][j]*Tfull)*f[j];      		
+			  }			
+			  theSolver->Solve(time,FF);    
+			  f[ii] = (*xdotdot);
+			  ff[ii] = (*xdot);
+		  }  	
+  
+	
+		  (*x) = initial_x + (c[0]*Tfull)*ff[0] + (c[2]*Tfull)*ff[2] + (c[3]*Tfull)*ff[3] + (c[5]*Tfull)*ff[5];  
+	
+		  (*xdot) = initial_xdot + (c[0]*Tfull)*f[0] + (c[2]*Tfull)*f[2] + (c[3]*Tfull)*f[3] + (c[5]*Tfull)*f[5];  
+	
+	
+		  int numjoints = system[i].system->joints.GetNumElements();
+		  for(int k = 0; k < numjoints; k++){
+			  system[i].system->joints(k)->ForwardKinematics();
+		  }		      
+						      	
+		
+		  for(int k = 0; k < numbodies; k++){
+			  Vect3 temp1 =system[i].system->bodies(k+1)->r;
+			  Vect3 temp2 =system[i].system->bodies(k+1)->v;
+			  Vect3 temp3 =system[i].system->bodies(k+1)->omega;
+			  Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;
+			  SysKE = SysKE + system[i].system->bodies(k+1)->KE;			  
+			  for(int m = 0; m < 3; m++){
+				  xcm[mappings[k]-1][m]   = temp1(m+1);
+				  vcm[mappings[k]-1][m]   = temp2(m+1);
+				  omega[mappings[k]-1][m] = temp3(m+1);
+				
+				  ex_space[mappings[k]-1][m] = temp4(m+1,1);				
+				  ey_space[mappings[k]-1][m] = temp4(m+1,2);
+				  ez_space[mappings[k]-1][m] = temp4(m+1,3);
+			  }         
+		  } 		
+		  delete theSolver;		      
+	  }	 	
+  }
+
+
+void Workspace::WriteFile(char * filename){
+	int numsys = currentIndex; 
+	int numbodies;				
+		  
+	
+	for (int i = 0; i <= numsys; i++){
+		numbodies = system[i].system->GetNumBodies() - 1;  	
+		ofstream outfile;
+		outfile.open(filename,ofstream::out | ios::app);		
+		outfile << numbodies<<endl;
+		outfile << "Atoms "<<endl;
+		for(int k = 0; k < numbodies; k++){
+			Vect3 temp1 =system[i].system->bodies(k+1)->r;
+			outfile<<1<<"\t"<<temp1(1)<<"\t"<<temp1(2)<<"\t"<<temp1(3)<<endl;
+		}
+		outfile.close();
+	}	
+  }

lib/poems/workspace.h

@@ -0,0 +1,88 @@
+/*
+ *_________________________________________________________________________*
+ *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
+ *      DESCRIPTION: SEE READ-ME                                           *
+ *      FILE NAME: workspace.h                                             *
+ *      AUTHORS: See Author List                                           * 
+ *      GRANTS: See Grants List                                            *
+ *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
+ *      LICENSE: Please see License Agreement                              *
+ *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
+ *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
+ *                     Computational Dynamics Lab                          *
+ *                     Rensselaer Polytechnic Institute                    *
+ *                     110 8th St. Troy NY 12180                           * 
+ *      CONTACT:        anderk5@rpi.edu                                    *
+ *_________________________________________________________________________*/
+
+
+#ifndef WORKSPACE_H
+#define WORKSPACE_H
+
+#include "matrices.h"
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <stdio.h>   
+#include <iomanip>  
+#include <vector>
+
+
+class System;
+class Solver;
+
+struct SysData{
+	System * system;
+	int solver;
+	int integrator;
+};
+
+class Workspace {
+	SysData * system; // the multibody systems data
+	int currentIndex;
+	int maxAlloc;
+	
+public:
+     Workspace();
+     ~Workspace();
+     
+     double Thalf;
+     double Tfull;
+     double ConFac;
+     double KE_val;
+	  int FirstTime;
+     
+     bool LoadFile(char* filename);
+     
+     bool SaveFile(char* filename, int index = -1);
+
+     System* GetSystem(int index = -1);
+     
+     void AddSolver(Solver* s, int index = -1);        
+    
+
+     void LobattoOne(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space);	
+     
+     void LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm);	 
+     
+ 
+     bool MakeSystem(int& nbody, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space, int &njoint, int **&jointbody, double **&xjoint, int& nfree, int*freelist, double dthalf, double dtv, double tempcon, double KE);
+     																																							
+	  
+	  bool SaveSystem(int& nbody, double *&masstotal, double **&inertia, double **&xcm, double **&xjoint, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space, double **&acm, double **&alpha, double **&torque, double **&fcm, int **&jointbody, int &njoint);
+	  																		
+	 bool MakeDegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space);
+     int getNumberOfSystems();
+     
+     void SetLammpsValues(double dtv, double dthalf, double tempcon);
+     void SetKE(int temp, double SysKE);
+	  
+	  void RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space);	
+	  
+	  void WriteFile(char* filename);
+
+private:
+	void allocateNewSystem(); //helper function to handle vector resizing and such for the array of system pointers
+};
+
+#endif