diff --git a/lib/README b/lib/README
new file mode 100644
index 000000000..540c05137
--- /dev/null
+++ b/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
diff --git a/lib/poems/Authors_List.txt b/lib/poems/Authors_List.txt
new file mode 100644
index 000000000..0a468fa2f
--- /dev/null
+++ b/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
diff --git a/lib/poems/Copyright_Notice b/lib/poems/Copyright_Notice
new file mode 100644
index 000000000..7c87c7fa3
--- /dev/null
+++ b/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.
\ No newline at end of file
diff --git a/lib/poems/Grants_List.txt b/lib/poems/Grants_List.txt
new file mode 100644
index 000000000..0fabf51ca
--- /dev/null
+++ b/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
+
\ No newline at end of file
diff --git a/lib/poems/Makefile b/lib/poems/Makefile
new file mode 100644
index 000000000..ed50802ff
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/lib/poems/Makefile.icc b/lib/poems/Makefile.icc
new file mode 100644
index 000000000..833a2f1d5
--- /dev/null
+++ b/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)
diff --git a/lib/poems/Makefile.storm b/lib/poems/Makefile.storm
new file mode 100644
index 000000000..8a3e01d36
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/lib/poems/POEMSChain.h b/lib/poems/POEMSChain.h
new file mode 100644
index 000000000..cc17952d0
--- /dev/null
+++ b/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
diff --git a/lib/poems/POEMS_License.txt b/lib/poems/POEMS_License.txt
new file mode 100644
index 000000000..9b54d2316
--- /dev/null
+++ b/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.
+
+
\ No newline at end of file
diff --git a/lib/poems/README b/lib/poems/README
new file mode 100644
index 000000000..f4043db58
--- /dev/null
+++ b/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
diff --git a/lib/poems/SystemProcessor.h b/lib/poems/SystemProcessor.h
new file mode 100644
index 000000000..fa43386a1
--- /dev/null
+++ b/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
diff --git a/lib/poems/bodies.h b/lib/poems/bodies.h
new file mode 100644
index 000000000..e47a4c80e
--- /dev/null
+++ b/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
diff --git a/lib/poems/body.cpp b/lib/poems/body.cpp
new file mode 100644
index 000000000..4ef05dd1e
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/lib/poems/body.h b/lib/poems/body.h
new file mode 100644
index 000000000..f8e0f6a5a
--- /dev/null
+++ b/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
diff --git a/lib/poems/body23joint.cpp b/lib/poems/body23joint.cpp
new file mode 100644
index 000000000..8cd0a95bc
--- /dev/null
+++ b/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);
+
+}
diff --git a/lib/poems/body23joint.h b/lib/poems/body23joint.h
new file mode 100644
index 000000000..60253ac8a
--- /dev/null
+++ b/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
diff --git a/lib/poems/colmatmap.cpp b/lib/poems/colmatmap.cpp
new file mode 100644
index 000000000..d2f631c20
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/colmatmap.h b/lib/poems/colmatmap.h
new file mode 100644
index 000000000..3a6267a24
--- /dev/null
+++ b/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
diff --git a/lib/poems/colmatrix.cpp b/lib/poems/colmatrix.cpp
new file mode 100644
index 000000000..abbb0181c
--- /dev/null
+++ b/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;
+ }
+}
+
diff --git a/lib/poems/colmatrix.h b/lib/poems/colmatrix.h
new file mode 100644
index 000000000..11fd85a92
--- /dev/null
+++ b/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
diff --git a/lib/poems/defines.h b/lib/poems/defines.h
new file mode 100644
index 000000000..55f94283d
--- /dev/null
+++ b/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
+
diff --git a/lib/poems/eulerparameters.cpp b/lib/poems/eulerparameters.cpp
new file mode 100644
index 000000000..61fcc5fcc
--- /dev/null
+++ b/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]);
+
+}
+
+
diff --git a/lib/poems/eulerparameters.h b/lib/poems/eulerparameters.h
new file mode 100644
index 000000000..029ade4b7
--- /dev/null
+++ b/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
+
diff --git a/lib/poems/fastmatrixops.cpp b/lib/poems/fastmatrixops.cpp
new file mode 100644
index 000000000..b887c9193
--- /dev/null
+++ b/lib/poems/fastmatrixops.cpp
@@ -0,0 +1,1033 @@
+/*
+ *_________________________________________________________________________*
+ * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
+ * DESCRIPTION: SEE READ-ME *
+ * FILE NAME: fastmatrixops.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 <iostream>
+#include "fastmatrixops.h"
+#include <cmath>
+
+using namespace std;
+
+//
+// Cross Product (friend of Vect3)
+//
+
+void FastCross(Vect3& a, Vect3& b, Vect3& c){
+ c.elements[0] = a.elements[1]*b.elements[2] - a.elements[2]*b.elements[1];
+ c.elements[1] = a.elements[2]*b.elements[0] - a.elements[0]*b.elements[2];
+ c.elements[2] = a.elements[0]*b.elements[1] - a.elements[1]*b.elements[0];
+}
+
+//
+// Simple Rotation (friend of Vect3 and Mat3x3)
+//
+
+void FastSimpleRotation(Vect3& v, double q, Mat3x3& C){
+ // intermediate quantities
+ double cq = cos(q);
+ double sq = sin(q);
+ double one_m_cq = 1-cq;
+ double l12 = v.elements[0]*v.elements[1]*one_m_cq;
+ double l13 = v.elements[0]*v.elements[2]*one_m_cq;
+ double l23 = v.elements[1]*v.elements[2]*one_m_cq;
+
+ // the transformation
+ C.elements[0][0] = v.elements[0]*v.elements[0]*one_m_cq+cq;
+ C.elements[0][1] = l12-v.elements[2]*sq;
+ C.elements[0][2] = l13+v.elements[1]*sq;
+ C.elements[1][0] = l12+v.elements[2]*sq;
+ C.elements[1][1] = v.elements[1]*v.elements[1]*one_m_cq+cq;
+ C.elements[1][2] = l23-v.elements[0]*sq;
+ C.elements[2][0] = l13-v.elements[1]*sq;
+ C.elements[2][1] = l23+v.elements[0]*sq;
+ C.elements[2][2] = v.elements[2]*v.elements[2]*one_m_cq+cq;
+}
+
+//
+// Quaternion Functions
+//
+
+void FastQuaternions(ColMatrix& q, Mat3x3& C){
+ double* e = q.elements;
+
+ // normalize the quaternions
+ double length = e[0]*e[0] + e[1]*e[1] + e[2]*e[2] + e[3]*e[3];
+ length = sqrt(length);
+ e[0] = e[0]/length;
+ e[1] = e[1]/length;
+ e[2] = e[2]/length;
+ e[3] = e[3]/length;
+
+ // compute the transformation
+ C.elements[0][0] = e[0]*e[0] + e[1]*e[1] - e[2]*e[2] - e[3]*e[3];
+ C.elements[1][1] = e[0]*e[0] - e[1]*e[1] + e[2]*e[2] - e[3]*e[3];
+ C.elements[2][2] = e[0]*e[0] - e[1]*e[1] - e[2]*e[2] + e[3]*e[3];
+
+ C.elements[0][1] = 2 * (e[1]*e[2] - e[0]*e[3]);
+ C.elements[0][2] = 2 * (e[1]*e[3] + e[0]*e[2]);
+ C.elements[1][2] = 2 * (e[2]*e[3] - e[0]*e[1]);
+
+ C.elements[1][0] = 2 * (e[1]*e[2] + e[0]*e[3]);
+ C.elements[2][0] = 2 * (e[1]*e[3] - e[0]*e[2]);
+ C.elements[2][1] = 2 * (e[2]*e[3] + e[0]*e[1]);
+}
+
+void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot){
+ double* w = omega.elements;
+ double* e = q.elements;
+
+ qdot.elements[0] = 0.5 * (-w[0]*e[1] - w[1]*e[2] - w[2]*e[3]);
+ qdot.elements[1] = 0.5 * ( w[0]*e[0] + w[2]*e[2] - w[1]*e[3]);
+ qdot.elements[2] = 0.5 * ( w[1]*e[0] - w[2]*e[1] + w[0]*e[3]);
+ qdot.elements[3] = 0.5 * ( w[2]*e[0] + w[1]*e[1] - w[0]*e[2]);
+}
+
+void FastInvQuaternions(Mat3x3& C, ColMatrix& q){
+}
+
+//
+// Inverse
+//
+
+// friend of Matrix
+//void FastInverse(Matrix& A, Matrix& C){ // C = A^(-1)
+// C.rows[0][0] = 1/A.rows[0][0];
+//}
+
+//
+// LDL^T Decomposition (from Golub and Van Loan)
+//
+
+// friend of Matrix
+void FastLDLT(Matrix& A, Matrix& C){ // C is the LD of the LDL^T decomposition of A (SPD)
+ double Lv;
+ int n = A.numrows;
+
+ for(int j=0;j<n;j++){
+ Lv = 0.0;
+ for(int i=0;i<j;i++){
+ C.rows[i][j] = C.rows[j][i]*C.rows[i][i];
+ Lv += C.rows[j][i]*C.rows[i][j];
+ }
+
+ C.rows[j][j] = A.rows[j][j] - Lv;
+ for(int i=j+1;i<n;i++){
+ Lv = 0.0;
+ for(int k=0;k<j;k++) Lv += C.rows[i][k]*C.rows[k][j];
+ C.rows[i][j] = (A.rows[i][j] - Lv)/C.rows[j][j];
+ }
+ }
+}
+
+
+// friend of Mat6x6
+void FastLDLT(Mat6x6& A, Mat6x6& C){ // C is the LD of the LDL^T decomposition of A (SPD)
+ double v[6];
+ double Lv;
+
+ for(int j=0;j<6;j++){
+ Lv = 0.0;
+ for(int i=0;i<j;i++){
+ v[i] = C.elements[j][i]*C.elements[i][i];
+ Lv += C.elements[j][i]*v[i];
+ }
+
+ v[j] = A.elements[j][j] - Lv;
+ C.elements[j][j] = v[j];
+ for(int i=j+1;i<6;i++){
+ Lv = 0.0;
+ for(int k=0;k<j;k++) Lv += C.elements[i][k]*v[k];
+ C.elements[i][j] = (A.elements[i][j] - Lv)/v[j];
+ }
+ }
+}
+
+// friend of Matrix
+void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C){
+ int n = B.numrows;
+ int c = B.numcols;
+ double temp;
+
+ for(int k=0;k<c;k++){
+ for(int i=0;i<n;i++){
+ temp = 0.0;
+ for(int j=0;j<i;j++){
+ temp += C.rows[j][k] * LD.rows[i][j];
+ }
+ C.rows[i][k] = B.rows[i][k] - temp;
+ }
+ for(int i=n-1;i>-1;i--){
+ C.rows[i][k] = C.rows[i][k] / LD.rows[i][i];
+ temp = 0.0;
+ for(int j=n-1;j>i;j--){
+ temp += C.rows[j][k] * LD.rows[j][i];
+ }
+ C.rows[i][k] = C.rows[i][k] - temp;
+ }
+ }
+}
+
+// friend of Matrix
+void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C){
+ int n = B.numcols;
+ int c = B.numrows;
+ double temp;
+
+ for(int k=0;k<c;k++){
+ for(int i=0;i<n;i++){
+ temp = 0.0;
+ for(int j=0;j<i;j++){
+ temp += C.rows[k][j] * LD.rows[i][j];
+ }
+ C.rows[k][i] = B.rows[k][i] - temp;
+ }
+ for(int i=n-1;i>-1;i--){
+ C.rows[k][i] = C.rows[k][i] / LD.rows[i][i];
+ temp = 0.0;
+ for(int j=n-1;j>i;j--){
+ temp += C.rows[k][j] * LD.rows[j][i];
+ }
+ C.rows[k][i] = C.rows[k][i] - temp;
+ }
+ }
+}
+
+// friend of Mat6x6
+void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C){
+ double temp;
+
+ for(int k=0;k<6;k++){
+ for(int i=0;i<6;i++){
+ temp = 0.0;
+ for(int j=0;j<i;j++){
+ temp += C.elements[j][k] * LD.elements[i][j];
+ }
+ C.elements[i][k] = B.elements[i][k] - temp;
+ }
+ for(int i=5;i>-1;i--){
+ C.elements[i][k] = C.elements[i][k] / LD.elements[i][i];
+ temp = 0.0;
+ for(int j=5;j>i;j--){
+ temp += C.elements[j][k] * LD.elements[j][i];
+ }
+ C.elements[i][k] = C.elements[i][k] - temp;
+ }
+ }
+}
+
+// friend of Mat6x6 & Vect6
+void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C){
+ double temp;
+
+ for(int i=0;i<6;i++){
+ temp = 0.0;
+ for(int j=0;j<i;j++){
+ temp += C.elements[j] * LD.elements[i][j];
+ }
+ C.elements[i] = B.elements[i] - temp;
+ }
+ for(int i=5;i>-1;i--){
+ C.elements[i] = C.elements[i] / LD.elements[i][i];
+ temp = 0.0;
+ for(int j=5;j>i;j--){
+ temp += C.elements[j] * LD.elements[j][i];
+ }
+ C.elements[i] = C.elements[i] - temp;
+ }
+}
+
+// friend of Matrix
+void FastLU(Matrix& A, Matrix& LU, int *indx){ // LU is the LU decomposition of A
+ int i,imax=0,j,k;
+ int n = A.numrows;
+ double big, dum, sum, temp;
+ double vv[10000];
+
+ LU = A;
+ for (i=0;i<n;i++){
+ big=0.0;
+ for (j=0;j<n;j++){
+ temp=fabs(LU.rows[i][j]);
+ if (temp > big) big=temp;
+ }
+ vv[i]=1.0/big;
+ }
+ for (j=0;j<n;j++){
+ for (i=0;i<j;i++){
+ sum=LU.rows[i][j];
+ for (k=0;k<i;k++) sum -= LU.rows[i][k]*LU.rows[k][j];
+ LU.rows[i][j]=sum;
+ }
+ big=0.0;
+ for (i=j;i<n;i++){
+ sum=LU.rows[i][j];
+ for (k=0;k<j;k++)
+ sum -= LU.rows[i][k]*LU.rows[k][j];
+ LU.rows[i][j]=sum;
+ if ((dum=vv[i]*fabs(sum)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<n;k++) {
+ dum=LU.rows[imax][k];
+ LU.rows[imax][k]=LU.rows[j][k];
+ LU.rows[j][k]=dum;
+ }
+ vv[imax]=vv[j];
+ }
+ indx[j]=imax;
+ // if (LU.rows[j][j] == 0.0) LU.rows[j][j]=1.0e-20;
+ if (j != n-1) {
+ dum=1.0/(LU.rows[j][j]);
+ for (i=j+1;i<n;i++) LU.rows[i][j] *= dum;
+ }
+ }
+}
+
+// friend of Mat3x3
+void FastLU(Mat3x3& A, Mat3x3& LU, int *indx){ // LU is the LU decomposition of A
+ int i,imax=0,j,k;
+ double big, dum, sum, temp;
+ double vv[10000];
+
+ LU = A;
+ for (i=0;i<3;i++){
+ big=0.0;
+ for (j=0;j<3;j++){
+ temp=fabs(LU.BasicGet(i,j));
+ if (temp > big) big=temp;
+ }
+ vv[i]=1.0/big;
+ }
+ for (j=0;j<3;j++){
+ for (i=0;i<j;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ }
+ big=0.0;
+ for (i=j;i<3;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<j;k++)
+ sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ if ((dum=vv[i]*fabs(sum)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<3;k++) {
+ dum=LU.BasicGet(imax,k);
+ LU.BasicSet(imax,k,LU.BasicGet(j,k));
+ LU.BasicSet(j,k,dum);
+ }
+ vv[imax]=vv[j];
+ }
+ indx[j]=imax;
+ if (j != 3-1) {
+ dum=1.0/(LU.BasicGet(j,j));
+ for (i=j+1;i<3;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
+ }
+ }
+}
+
+// friend of Mat4x4
+void FastLU(Mat4x4& A, Mat4x4& LU, int *indx){ // LU is the LU decomposition of A
+ int i,imax=0,j,k;
+ double big, dum, sum, temp;
+ double vv[10000];
+
+ LU = A;
+ for (i=0;i<4;i++){
+ big=0.0;
+ for (j=0;j<4;j++){
+ temp=fabs(LU.BasicGet(i,j));
+ if (temp > big) big=temp;
+ }
+ vv[i]=1.0/big;
+ }
+ for (j=0;j<4;j++){
+ for (i=0;i<j;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ }
+ big=0.0;
+ for (i=j;i<4;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<j;k++)
+ sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ if ((dum=vv[i]*fabs(sum)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<4;k++) {
+ dum=LU.BasicGet(imax,k);
+ LU.BasicSet(imax,k,LU.BasicGet(j,k));
+ LU.BasicSet(j,k,dum);
+ }
+ vv[imax]=vv[j];
+ }
+ indx[j]=imax;
+ if (j != 4-1) {
+ dum=1.0/(LU.BasicGet(j,j));
+ for (i=j+1;i<4;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
+ }
+ }
+}
+
+// friend of Mat6x6
+void FastLU(Mat6x6& A, Mat6x6& LU, int *indx){ // LU is the LU decomposition of A
+ int i,imax=0,j,k;
+ double big, dum, sum, temp;
+ double vv[10000];
+
+ LU = A;
+ for (i=0;i<6;i++){
+ big=0.0;
+ for (j=0;j<6;j++){
+ temp=fabs(LU.BasicGet(i,j));
+ if (temp > big) big=temp;
+ }
+ vv[i]=1.0/big;
+ }
+ for (j=0;j<6;j++){
+ for (i=0;i<j;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<i;k++) sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ }
+ big=0.0;
+ for (i=j;i<6;i++){
+ sum=LU.BasicGet(i,j);
+ for (k=0;k<j;k++)
+ sum -= LU.BasicGet(i,k)*LU.BasicGet(k,j);
+ LU.BasicSet(i,j,sum);
+ if ((dum=vv[i]*fabs(sum)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<6;k++) {
+ dum=LU.BasicGet(imax,k);
+ LU.BasicSet(imax,k,LU.BasicGet(j,k));
+ LU.BasicSet(j,k,dum);
+ }
+ vv[imax]=vv[j];
+ }
+ indx[j]=imax;
+ if (j != 6-1) {
+ dum=1.0/(LU.BasicGet(j,j));
+ for (i=j+1;i<6;i++) LU.BasicSet(i,j,LU.BasicGet(i,j)*dum);
+ }
+ }
+}
+
+// friend of Matrix
+void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
+ int i,ip,j,k;
+ int n = B.numrows;
+ int c = B.numcols;
+ double temp;
+
+ C = B;
+ for (k=0;k<c;k++){
+ for (i=0;i<n;i++){
+ ip=indx[i];
+ temp=C.rows[ip][k];
+ C.rows[ip][k]=C.rows[i][k];
+ for (j=0;j<i;j++) temp -= LU.rows[i][j]*C.rows[j][k];
+ C.rows[i][k]=temp;
+ }
+ for (i=n-1;i>=0;i--){
+ temp=C.rows[i][k];
+ for (j=i+1;j<n;j++) temp -= LU.rows[i][j]*C.rows[j][k];
+ C.rows[i][k]=temp/LU.rows[i][i];
+ }
+ }
+}
+
+// friend of Matrix and Mat3x3
+void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
+ int i,ip,j,k;
+ int n = B.numrows;
+ int c = B.numcols;
+ double temp;
+
+ C = B;
+ for (k=0;k<c;k++){
+ for (i=0;i<n;i++){
+ ip=indx[i];
+ temp=C.rows[ip][k];
+ C.rows[ip][k]=C.rows[i][k];
+ for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp;
+ }
+ for (i=n-1;i>=0;i--){
+ temp=C.rows[i][k];
+ for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp/LU.BasicGet(i,i);
+ }
+ }
+}
+
+// friend of Matrix and Mat4x4
+void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
+ int i,ip,j,k;
+ int n = B.numrows;
+ int c = B.numcols;
+ double temp;
+
+ C = B;
+ for (k=0;k<c;k++){
+ for (i=0;i<n;i++){
+ ip=indx[i];
+ temp=C.rows[ip][k];
+ C.rows[ip][k]=C.rows[i][k];
+ for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp;
+ }
+ for (i=n-1;i>=0;i--){
+ temp=C.rows[i][k];
+ for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp/LU.BasicGet(i,i);
+ }
+ }
+}
+
+// friend of Matrix and Mat6x6
+void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
+ int i,ip,j,k;
+ int n = B.numrows;
+ int c = B.numcols;
+ double temp;
+
+ C = B;
+ for (k=0;k<c;k++){
+ for (i=0;i<n;i++){
+ ip=indx[i];
+ temp=C.rows[ip][k];
+ C.rows[ip][k]=C.rows[i][k];
+ for (j=0;j<i;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp;
+ }
+ for (i=n-1;i>=0;i--){
+ temp=C.rows[i][k];
+ for (j=i+1;j<n;j++) temp -= LU.BasicGet(i,j)*C.rows[j][k];
+ C.rows[i][k]=temp/LU.BasicGet(i,i);
+ }
+ }
+}
+
+
+// The following LUSubsLH routine is incomplete at the moment.
+// friend of Matrix
+void FastLUSubsLH(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
+ int i,ip,j,k;
+ int n = B.numcols;
+ int c = B.numrows;
+ double temp;
+ Matrix C_temp;
+
+ C_temp = B;
+ for (k=0;k<c;k++){
+ for (i=0;i<n;i++){
+ ip=indx[k];
+ temp=C_temp.rows[ip][i];
+ C_temp.rows[ip][i]=C_temp.rows[k][i];
+ for (j=0;j<i;j++) temp -= LU.rows[i][j]*C_temp.rows[k][j];
+ C_temp.rows[k][i]=temp;
+ }
+ for (i=n-1;i>=0;i--){
+ temp=C_temp.rows[k][i];
+ for (j=i+1;j<n;j++) temp -= LU.rows[i][j]*C_temp.rows[k][j];
+ C_temp.rows[k][i]=temp/LU.rows[i][i];
+ }
+ }
+ C = C_temp;
+}
+
+
+
+//
+// Triple sum
+//
+
+void FastTripleSum(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b+c
+ d.elements[0] = a.elements[0]+b.elements[0]+c.elements[0];
+ d.elements[1] = a.elements[1]+b.elements[1]+c.elements[1];
+ d.elements[2] = a.elements[2]+b.elements[2]+c.elements[2];
+}
+
+void FastTripleSumPPM(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b-c
+ d.elements[0] = a.elements[0]+b.elements[0]-c.elements[0];
+ d.elements[1] = a.elements[1]+b.elements[1]-c.elements[1];
+ d.elements[2] = a.elements[2]+b.elements[2]-c.elements[2];
+}
+
+//
+// Multiplications
+//
+
+// friend of matrix
+void FastMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
+ // assumes dimensions are already correct!
+ int r = A.numrows;
+ int ca = A.numcols;
+ int cb = B.numcols;
+
+ int i,j,k;
+ for(i=0;i<r;i++)
+ for(j=0;j<cb;j++){
+ C.rows[i][j] = 0.0;
+ for(k=0;k<ca;k++)
+ C.rows[i][j] += A.rows[i][k] * B.rows[k][j];
+ }
+}
+
+// friend of matrix
+void FastTMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
+ // assumes dimensions are already correct!
+ int r = A.numcols;
+ int ca = A.numrows;
+ int cb = B.numcols;
+
+ int i,j,k;
+ for(i=0;i<r;i++)
+ for(j=0;j<cb;j++){
+ C.rows[i][j] = A.rows[0][i] * B.rows[0][j];
+ for(k=1;k<ca;k++)
+ C.rows[i][j] += A.rows[k][i] * B.rows[k][j];
+ }
+}
+
+// friend of Mat3x3 & Vect3
+void FastMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = A*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
+ C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
+ C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
+}
+
+// friend of Mat3x3, ColMatrix, & Vect3
+void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C){ // C = A*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
+ C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
+ C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
+}
+
+
+// friend of Mat3x3, ColMatrix, & Vect3
+void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C){ // C = A*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2];
+ C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2];
+ C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
+}
+
+// friend of Mat3x3 & Vect3
+void FastTMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = A^T*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[1][0]*B.elements[1] + A.elements[2][0]*B.elements[2];
+ C.elements[1] = A.elements[0][1]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[2][1]*B.elements[2];
+ C.elements[2] = A.elements[0][2]*B.elements[0] + A.elements[1][2]*B.elements[1] + A.elements[2][2]*B.elements[2];
+}
+
+// friend of Mat3x3 & Vect3
+void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = -A*B
+ C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[0][1]*B.elements[1] - A.elements[0][2]*B.elements[2];
+ C.elements[1] = -A.elements[1][0]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[1][2]*B.elements[2];
+ C.elements[2] = -A.elements[2][0]*B.elements[0] - A.elements[2][1]*B.elements[1] - A.elements[2][2]*B.elements[2];
+}
+
+// friend of Mat3x3 & Vect3
+void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = -A^T*B
+ C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[1][0]*B.elements[1] - A.elements[2][0]*B.elements[2];
+ C.elements[1] = -A.elements[0][1]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[2][1]*B.elements[2];
+ C.elements[2] = -A.elements[0][2]*B.elements[0] - A.elements[1][2]*B.elements[1] - A.elements[2][2]*B.elements[2];
+}
+
+// friend of Vect3
+void FastMult(double a, Vect3& B, Vect3& C){ // C = a*B
+ C.elements[0] = a*B.elements[0];
+ C.elements[1] = a*B.elements[1];
+ C.elements[2] = a*B.elements[2];
+}
+
+// friend of Mat4x4 & Vect4
+void FastMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[0][1]*B.elements[1] + A.elements[0][2]*B.elements[2] + A.elements[0][3]*B.elements[3];
+ C.elements[1] = A.elements[1][0]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[1][2]*B.elements[2] + A.elements[1][3]*B.elements[3];
+ C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2] + A.elements[2][3]*B.elements[3];
+ C.elements[3] = A.elements[3][0]*B.elements[0] + A.elements[3][1]*B.elements[1] + A.elements[3][2]*B.elements[2] + A.elements[3][3]*B.elements[3];
+}
+
+// friend of Mat4x4 & Vect4
+void FastTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A^T*B
+ C.elements[0] = A.elements[0][0]*B.elements[0] + A.elements[1][0]*B.elements[1] + A.elements[2][0]*B.elements[2] + A.elements[3][0]*B.elements[3];
+ C.elements[1] = A.elements[0][1]*B.elements[0] + A.elements[1][1]*B.elements[1] + A.elements[2][1]*B.elements[2] + A.elements[3][1]*B.elements[3];
+ C.elements[2] = A.elements[0][2]*B.elements[0] + A.elements[1][2]*B.elements[1] + A.elements[2][2]*B.elements[2] + A.elements[3][2]*B.elements[3];
+ C.elements[3] = A.elements[0][3]*B.elements[0] + A.elements[1][3]*B.elements[1] + A.elements[2][3]*B.elements[2] + A.elements[3][3]*B.elements[3];
+}
+
+// friend of Mat4x4 & Vect4
+void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A*B
+ C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[0][1]*B.elements[1] - A.elements[0][2]*B.elements[2] - A.elements[0][3]*B.elements[3];
+ C.elements[1] = -A.elements[1][0]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[1][2]*B.elements[2] - A.elements[1][3]*B.elements[3];
+ C.elements[2] = -A.elements[2][0]*B.elements[0] - A.elements[2][1]*B.elements[1] - A.elements[2][2]*B.elements[2] - A.elements[2][3]*B.elements[3];
+ C.elements[3] = -A.elements[3][0]*B.elements[0] - A.elements[3][1]*B.elements[1] - A.elements[3][2]*B.elements[2] - A.elements[3][3]*B.elements[3];
+}
+
+// friend of Mat4x4 & Vect4
+void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A^T*B
+ C.elements[0] = -A.elements[0][0]*B.elements[0] - A.elements[1][0]*B.elements[1] - A.elements[2][0]*B.elements[2] - A.elements[3][0]*B.elements[3];
+ C.elements[1] = -A.elements[0][1]*B.elements[0] - A.elements[1][1]*B.elements[1] - A.elements[2][1]*B.elements[2] - A.elements[3][1]*B.elements[3];
+ C.elements[2] = -A.elements[0][2]*B.elements[0] - A.elements[1][2]*B.elements[1] - A.elements[2][2]*B.elements[2] - A.elements[3][2]*B.elements[3];
+ C.elements[3] = -A.elements[0][3]*B.elements[0] - A.elements[1][3]*B.elements[1] - A.elements[2][3]*B.elements[2] - A.elements[3][3]*B.elements[3];
+}
+
+// friend of Vect4
+void FastMult(double a, Vect4& B, Vect4& C){ // C = a*B
+ C.elements[0] = a*B.elements[0];
+ C.elements[1] = a*B.elements[1];
+ C.elements[2] = a*B.elements[2];
+ C.elements[3] = a*B.elements[3];
+}
+
+// friend of Matrix & Mat6x6
+void FastMultT(Matrix& A, Matrix& B, Mat6x6& C){ // C = A*B^T
+ int i,j,k,n;
+ n = A.numcols;
+
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++){
+ C.elements[i][j] = 0.0;
+ for(k=0;k<n;k++)
+ C.elements[i][j] += A.rows[i][k] * B.rows[j][k];
+ }
+}
+
+// friend Matrix, Vect6, ColMatrix
+void FastMult(Matrix& A, ColMatrix& B, Vect6& C){
+ int ca = A.numcols;
+
+ int i,k;
+ for(i=0;i<6;i++){
+ C.elements[i] = 0.0;
+ for(k=0;k<ca;k++)
+ C.elements[i] += A.rows[i][k] * B.elements[k];
+ }
+}
+
+// friend of Matrix & Mat6x6
+void FastMult(Mat6x6& A, Matrix& B, Matrix& C){ // C = A*B
+ // assumes dimensions are already correct!
+ int cb = B.numcols;
+
+ int i,j,k;
+ for(i=0;i<6;i++)
+ for(j=0;j<cb;j++){
+ C.rows[i][j] = 0.0;
+ for(k=0;k<6;k++)
+ C.rows[i][j] += A.elements[i][k] * B.rows[k][j];
+ }
+}
+
+// friend Matrix, Vect6, ColMatrix
+void FastTMult(Matrix& A, Vect6& B, ColMatrix& C){ // C = A^T*B
+ int n = C.numrows;
+ int i,k;
+ for(i=0;i<n;i++){
+ C.elements[i] = 0.0;
+ for(k=0;k<6;k++)
+ C.elements[i] += A.rows[k][i] * B.elements[k];
+ }
+}
+
+// friend of Mat3x3
+void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B
+ C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[1][0] + A.elements[0][2]*B.elements[2][0];
+ C.elements[0][1] = A.elements[0][0]*B.elements[0][1] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[2][1];
+ C.elements[0][2] = A.elements[0][0]*B.elements[0][2] + A.elements[0][1]*B.elements[1][2] + A.elements[0][2]*B.elements[2][2];
+
+ C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[1][0] + A.elements[1][2]*B.elements[2][0];
+ C.elements[1][1] = A.elements[1][0]*B.elements[0][1] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[2][1];
+ C.elements[1][2] = A.elements[1][0]*B.elements[0][2] + A.elements[1][1]*B.elements[1][2] + A.elements[1][2]*B.elements[2][2];
+
+ C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[1][0] + A.elements[2][2]*B.elements[2][0];
+ C.elements[2][1] = A.elements[2][0]*B.elements[0][1] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[2][1];
+ C.elements[2][2] = A.elements[2][0]*B.elements[0][2] + A.elements[2][1]*B.elements[1][2] + A.elements[2][2]*B.elements[2][2];
+}
+
+// friend of Mat3x3
+void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B^T
+ C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[0][1] + A.elements[0][2]*B.elements[0][2];
+ C.elements[0][1] = A.elements[0][0]*B.elements[1][0] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[1][2];
+ C.elements[0][2] = A.elements[0][0]*B.elements[2][0] + A.elements[0][1]*B.elements[2][1] + A.elements[0][2]*B.elements[2][2];
+
+ C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[0][1] + A.elements[1][2]*B.elements[0][2];
+ C.elements[1][1] = A.elements[1][0]*B.elements[1][0] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[1][2];
+ C.elements[1][2] = A.elements[1][0]*B.elements[2][0] + A.elements[1][1]*B.elements[2][1] + A.elements[1][2]*B.elements[2][2];
+
+ C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[0][1] + A.elements[2][2]*B.elements[0][2];
+ C.elements[2][1] = A.elements[2][0]*B.elements[1][0] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[1][2];
+ C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2];
+}
+
+// friend of Mat4x4
+void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B
+ C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[1][0] + A.elements[0][2]*B.elements[2][0] + A.elements[0][3]*B.elements[3][0];
+ C.elements[0][1] = A.elements[0][0]*B.elements[0][1] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[2][1] + A.elements[0][3]*B.elements[3][1];
+ C.elements[0][2] = A.elements[0][0]*B.elements[0][2] + A.elements[0][1]*B.elements[1][2] + A.elements[0][2]*B.elements[2][2] + A.elements[0][3]*B.elements[3][2];
+ C.elements[0][3] = A.elements[0][0]*B.elements[0][3] + A.elements[0][1]*B.elements[1][3] + A.elements[0][2]*B.elements[2][3] + A.elements[0][3]*B.elements[3][3];
+
+ C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[1][0] + A.elements[1][2]*B.elements[2][0] + A.elements[1][3]*B.elements[3][0];
+ C.elements[1][1] = A.elements[1][0]*B.elements[0][1] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[2][1] + A.elements[1][3]*B.elements[3][1];
+ C.elements[1][2] = A.elements[1][0]*B.elements[0][2] + A.elements[1][1]*B.elements[1][2] + A.elements[1][2]*B.elements[2][2] + A.elements[1][3]*B.elements[3][2];
+ C.elements[1][3] = A.elements[1][0]*B.elements[0][3] + A.elements[1][1]*B.elements[1][3] + A.elements[1][2]*B.elements[2][3] + A.elements[1][3]*B.elements[3][3];
+
+ C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[1][0] + A.elements[2][2]*B.elements[2][0] + A.elements[2][3]*B.elements[3][0];
+ C.elements[2][1] = A.elements[2][0]*B.elements[0][1] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[2][1] + A.elements[2][3]*B.elements[3][1];
+ C.elements[2][2] = A.elements[2][0]*B.elements[0][2] + A.elements[2][1]*B.elements[1][2] + A.elements[2][2]*B.elements[2][2] + A.elements[2][3]*B.elements[3][2];
+ C.elements[2][3] = A.elements[2][0]*B.elements[0][3] + A.elements[2][1]*B.elements[1][3] + A.elements[2][2]*B.elements[2][3] + A.elements[2][3]*B.elements[3][3];
+
+ C.elements[3][0] = A.elements[3][0]*B.elements[0][0] + A.elements[3][1]*B.elements[1][0] + A.elements[3][2]*B.elements[2][0] + A.elements[3][3]*B.elements[3][0];
+ C.elements[3][1] = A.elements[3][0]*B.elements[0][1] + A.elements[3][1]*B.elements[1][1] + A.elements[3][2]*B.elements[2][1] + A.elements[3][3]*B.elements[3][1];
+ C.elements[3][2] = A.elements[3][0]*B.elements[0][2] + A.elements[3][1]*B.elements[1][2] + A.elements[3][2]*B.elements[2][2] + A.elements[3][3]*B.elements[3][2];
+ C.elements[3][3] = A.elements[3][0]*B.elements[0][3] + A.elements[3][1]*B.elements[1][3] + A.elements[3][2]*B.elements[2][3] + A.elements[3][3]*B.elements[3][3];
+}
+
+// friend of Mat4x4
+void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B^T
+ C.elements[0][0] = A.elements[0][0]*B.elements[0][0] + A.elements[0][1]*B.elements[0][1] + A.elements[0][2]*B.elements[0][2] + A.elements[0][3]*B.elements[0][3];
+ C.elements[0][1] = A.elements[0][0]*B.elements[1][0] + A.elements[0][1]*B.elements[1][1] + A.elements[0][2]*B.elements[1][2] + A.elements[0][3]*B.elements[1][3];
+ C.elements[0][2] = A.elements[0][0]*B.elements[2][0] + A.elements[0][1]*B.elements[2][1] + A.elements[0][2]*B.elements[2][2] + A.elements[0][3]*B.elements[2][3];
+ C.elements[0][3] = A.elements[0][0]*B.elements[3][0] + A.elements[0][1]*B.elements[3][1] + A.elements[0][2]*B.elements[3][2] + A.elements[0][3]*B.elements[3][3];
+
+ C.elements[1][0] = A.elements[1][0]*B.elements[0][0] + A.elements[1][1]*B.elements[0][1] + A.elements[1][2]*B.elements[0][2] + A.elements[1][3]*B.elements[0][3];
+ C.elements[1][1] = A.elements[1][0]*B.elements[1][0] + A.elements[1][1]*B.elements[1][1] + A.elements[1][2]*B.elements[1][2] + A.elements[1][3]*B.elements[1][3];
+ C.elements[1][2] = A.elements[1][0]*B.elements[2][0] + A.elements[1][1]*B.elements[2][1] + A.elements[1][2]*B.elements[2][2] + A.elements[1][3]*B.elements[2][3];
+ C.elements[1][3] = A.elements[1][0]*B.elements[3][0] + A.elements[1][1]*B.elements[3][1] + A.elements[1][2]*B.elements[3][2] + A.elements[1][3]*B.elements[3][3];
+
+ C.elements[2][0] = A.elements[2][0]*B.elements[0][0] + A.elements[2][1]*B.elements[0][1] + A.elements[2][2]*B.elements[0][2] + A.elements[2][3]*B.elements[0][3];
+ C.elements[2][1] = A.elements[2][0]*B.elements[1][0] + A.elements[2][1]*B.elements[1][1] + A.elements[2][2]*B.elements[1][2] + A.elements[2][3]*B.elements[1][3];
+ C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2] + A.elements[2][3]*B.elements[2][3];
+ C.elements[2][3] = A.elements[2][0]*B.elements[3][0] + A.elements[2][1]*B.elements[3][1] + A.elements[2][2]*B.elements[3][2] + A.elements[2][3]*B.elements[3][3];
+
+ C.elements[3][0] = A.elements[3][0]*B.elements[0][0] + A.elements[3][1]*B.elements[0][1] + A.elements[3][2]*B.elements[0][2] + A.elements[3][3]*B.elements[0][3];
+ C.elements[3][1] = A.elements[3][0]*B.elements[1][0] + A.elements[3][1]*B.elements[1][1] + A.elements[3][2]*B.elements[1][2] + A.elements[3][3]*B.elements[1][3];
+ C.elements[3][2] = A.elements[3][0]*B.elements[2][0] + A.elements[3][1]*B.elements[2][1] + A.elements[3][2]*B.elements[2][2] + A.elements[3][3]*B.elements[2][3];
+ C.elements[3][3] = A.elements[3][0]*B.elements[3][0] + A.elements[3][1]*B.elements[3][1] + A.elements[3][2]*B.elements[3][2] + A.elements[3][3]*B.elements[3][3];
+ C.elements[2][2] = A.elements[2][0]*B.elements[2][0] + A.elements[2][1]*B.elements[2][1] + A.elements[2][2]*B.elements[2][2];
+}
+
+// friend of Mat6x6
+void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
+ int i,j,k;
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++){
+ C.elements[i][j] = 0.0;
+ for(k=0;k<6;k++)
+ C.elements[i][j] += A.elements[i][k]*B.elements[k][j];
+ }
+}
+
+// friend of Mat6x6
+void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
+ int i,j,k;
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++){
+ C.elements[i][j] = 0.0;
+ for(k=0;k<6;k++)
+ C.elements[i][j] += A.elements[i][k]*B.elements[j][k];
+ }
+}
+
+// friend of Mat6x6
+void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A^T*B
+ int i,j,k;
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++){
+ C.elements[i][j] = 0.0;
+ for(k=0;k<6;k++)
+ C.elements[i][j] += A.elements[k][i]*B.elements[k][j];
+ }
+}
+
+// friend of Mat6x6 & Vect6
+void FastMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A*B
+ for(int i=0;i<6;i++)
+ C.elements[i] = A.elements[i][0]*B.elements[0] + A.elements[i][1]*B.elements[1] + A.elements[i][2]*B.elements[2] + A.elements[i][3]*B.elements[3] + A.elements[i][4]*B.elements[4] + A.elements[i][5]*B.elements[5];
+}
+
+// friend of Mat6x6 & Vect6
+void FastTMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A^T*B
+ for(int i=0;i<6;i++)
+ C.elements[i] = A.elements[0][i]*B.elements[0] + A.elements[1][i]*B.elements[1] + A.elements[2][i]*B.elements[2] + A.elements[3][i]*B.elements[3] + A.elements[4][i]*B.elements[4] + A.elements[5][i]*B.elements[5];
+}
+
+//
+// Additions
+//
+
+// friend of Vect3
+void FastAdd(Vect3& A, Vect3& B, Vect3& C){ // C = A+B
+ C.elements[0] = A.elements[0] + B.elements[0];
+ C.elements[1] = A.elements[1] + B.elements[1];
+ C.elements[2] = A.elements[2] + B.elements[2];
+}
+
+// friend of Vect4
+void FastAdd(Vect4& A, Vect4& B, Vect4& C){ // C = A+B
+ C.elements[0] = A.elements[0] + B.elements[0];
+ C.elements[1] = A.elements[1] + B.elements[1];
+ C.elements[2] = A.elements[2] + B.elements[2];
+ C.elements[3] = A.elements[3] + B.elements[3];
+}
+
+void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A+B
+ int i,j;
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++)
+ C.elements[i][j] = A.elements[i][j] + B.elements[i][j];
+}
+
+// friend of Vect6
+void FastAdd(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
+ C.elements[0] = A.elements[0] + B.elements[0];
+ C.elements[1] = A.elements[1] + B.elements[1];
+ C.elements[2] = A.elements[2] + B.elements[2];
+ C.elements[3] = A.elements[3] + B.elements[3];
+ C.elements[4] = A.elements[4] + B.elements[4];
+ C.elements[5] = A.elements[5] + B.elements[5];
+}
+
+//
+// Subtractions
+//
+
+// friend of Vect3
+void FastSubt(Vect3& A, Vect3& B, Vect3& C){ // C = A-B
+ C.elements[0] = A.elements[0] - B.elements[0];
+ C.elements[1] = A.elements[1] - B.elements[1];
+ C.elements[2] = A.elements[2] - B.elements[2];
+}
+
+// friend of Vect4
+void FastSubt(Vect4& A, Vect4& B, Vect4& C){ // C = A-B
+ C.elements[0] = A.elements[0] - B.elements[0];
+ C.elements[1] = A.elements[1] - B.elements[1];
+ C.elements[2] = A.elements[2] - B.elements[2];
+ C.elements[3] = A.elements[3] - B.elements[3];
+}
+
+void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A-B
+ int i,j;
+ for(i=0;i<6;i++)
+ for(j=0;j<6;j++)
+ C.elements[i][j] = A.elements[i][j] - B.elements[i][j];
+}
+
+// friend of Vect6
+void FastSubt(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
+ C.elements[0] = A.elements[0] - B.elements[0];
+ C.elements[1] = A.elements[1] - B.elements[1];
+ C.elements[2] = A.elements[2] - B.elements[2];
+ C.elements[3] = A.elements[3] - B.elements[3];
+ C.elements[4] = A.elements[4] - B.elements[4];
+ C.elements[5] = A.elements[5] - B.elements[5];
+}
+
+// friend of ColMatMap
+void FastAssign(ColMatMap& A, ColMatMap& C){
+ for(int i=0;i<C.numrows;i++)
+ *(C.elements[i]) = *(A.elements[i]);
+}
+
+// friend of ColMatrix
+void FastAssign(ColMatrix& A, ColMatrix& C){ //C = A
+ for(int i=0;i<C.numrows;i++)
+ C.elements[i] = A.elements[i];
+}
+
+// friend of Vect3
+void FastAssign(Vect3& A, Vect3& C){ //C = A
+ C.elements[0] = A.elements[0];
+ C.elements[1] = A.elements[1];
+ C.elements[2] = A.elements[2];
+}
+
+// friend of Vect3 & ColMatrix
+void FastAssign(ColMatrix& A, Vect3& C){ //C = A
+ C.elements[0] = A.elements[0];
+ C.elements[1] = A.elements[1];
+ C.elements[2] = A.elements[2];
+}
+
+// friend of Vect4
+void FastAssign(Vect4& A, Vect4& C){ //C = A
+ C.elements[0] = A.elements[0];
+ C.elements[1] = A.elements[1];
+ C.elements[2] = A.elements[2];
+ C.elements[3] = A.elements[3];
+}
+
+// friend of Mat3x3
+void FastAssignT(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[1][0] = A.elements[0][1];
+
+ C.elements[0][2] = A.elements[2][0];
+ C.elements[2][0] = A.elements[0][2];
+
+ C.elements[1][2] = A.elements[2][1];
+ C.elements[2][1] = A.elements[1][2];
+}
+
+// friend of Mat4x4
+void FastAssignT(Mat4x4& A, Mat4x4& 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[3][3] = A.elements[3][3];
+
+ C.elements[0][1] = A.elements[1][0];
+ C.elements[1][0] = A.elements[0][1];
+
+ C.elements[0][2] = A.elements[2][0];
+ C.elements[2][0] = A.elements[0][2];
+
+ C.elements[0][3] = A.elements[3][0];
+ C.elements[3][0] = A.elements[0][3];
+
+ C.elements[1][2] = A.elements[2][1];
+ C.elements[2][1] = A.elements[1][2];
+
+ C.elements[1][3] = A.elements[3][1];
+ C.elements[3][1] = A.elements[1][3];
+
+ C.elements[2][3] = A.elements[3][2];
+ C.elements[3][2] = A.elements[2][3];
+}
+
diff --git a/lib/poems/fastmatrixops.h b/lib/poems/fastmatrixops.h
new file mode 100644
index 000000000..e7ccdbd0c
--- /dev/null
+++ b/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
diff --git a/lib/poems/fixedpoint.cpp b/lib/poems/fixedpoint.cpp
new file mode 100644
index 000000000..860088239
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/fixedpoint.h b/lib/poems/fixedpoint.h
new file mode 100644
index 000000000..b5ecb2390
--- /dev/null
+++ b/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
diff --git a/lib/poems/freebodyjoint.cpp b/lib/poems/freebodyjoint.cpp
new file mode 100644
index 000000000..d00f0bdb9
--- /dev/null
+++ b/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;
+
+}
diff --git a/lib/poems/freebodyjoint.h b/lib/poems/freebodyjoint.h
new file mode 100644
index 000000000..27cacf7dc
--- /dev/null
+++ b/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
diff --git a/lib/poems/inertialframe.cpp b/lib/poems/inertialframe.cpp
new file mode 100644
index 000000000..6c97b2f58
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/inertialframe.h b/lib/poems/inertialframe.h
new file mode 100644
index 000000000..08d0279a1
--- /dev/null
+++ b/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
diff --git a/lib/poems/joint.cpp b/lib/poems/joint.cpp
new file mode 100644
index 000000000..3bdd7b5fc
--- /dev/null
+++ b/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
+ }
+}
diff --git a/lib/poems/joint.h b/lib/poems/joint.h
new file mode 100644
index 000000000..17122af77
--- /dev/null
+++ b/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
diff --git a/lib/poems/joints.h b/lib/poems/joints.h
new file mode 100644
index 000000000..240a272bd
--- /dev/null
+++ b/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
diff --git a/lib/poems/mat3x3.cpp b/lib/poems/mat3x3.cpp
new file mode 100644
index 000000000..56ff27035
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/mat3x3.h b/lib/poems/mat3x3.h
new file mode 100644
index 000000000..4362b3ad9
--- /dev/null
+++ b/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
diff --git a/lib/poems/mat4x4.cpp b/lib/poems/mat4x4.cpp
new file mode 100644
index 000000000..a6a4a77fb
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/mat4x4.h b/lib/poems/mat4x4.h
new file mode 100644
index 000000000..03637646f
--- /dev/null
+++ b/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
diff --git a/lib/poems/mat6x6.cpp b/lib/poems/mat6x6.cpp
new file mode 100644
index 000000000..84bda1faf
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/mat6x6.h b/lib/poems/mat6x6.h
new file mode 100644
index 000000000..11951b044
--- /dev/null
+++ b/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
diff --git a/lib/poems/matrices.h b/lib/poems/matrices.h
new file mode 100644
index 000000000..934a21b4e
--- /dev/null
+++ b/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
diff --git a/lib/poems/matrix.cpp b/lib/poems/matrix.cpp
new file mode 100644
index 000000000..68275718f
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/matrix.h b/lib/poems/matrix.h
new file mode 100644
index 000000000..63699b983
--- /dev/null
+++ b/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
diff --git a/lib/poems/matrixfun.cpp b/lib/poems/matrixfun.cpp
new file mode 100644
index 000000000..80a9450bf
--- /dev/null
+++ b/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];
+}
+
diff --git a/lib/poems/matrixfun.h b/lib/poems/matrixfun.h
new file mode 100644
index 000000000..600b608ad
--- /dev/null
+++ b/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
diff --git a/lib/poems/mixedjoint.cpp b/lib/poems/mixedjoint.cpp
new file mode 100644
index 000000000..c834ef621
--- /dev/null
+++ b/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;
+
+}
diff --git a/lib/poems/mixedjoint.h b/lib/poems/mixedjoint.h
new file mode 100644
index 000000000..5f59b95f0
--- /dev/null
+++ b/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
diff --git a/lib/poems/norm.cpp b/lib/poems/norm.cpp
new file mode 100644
index 000000000..0118476e8
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/norm.h b/lib/poems/norm.h
new file mode 100644
index 000000000..974c1768a
--- /dev/null
+++ b/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
diff --git a/lib/poems/onbody.cpp b/lib/poems/onbody.cpp
new file mode 100644
index 000000000..102900df8
--- /dev/null
+++ b/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(){
+}
diff --git a/lib/poems/onbody.h b/lib/poems/onbody.h
new file mode 100644
index 000000000..d9923773c
--- /dev/null
+++ b/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
diff --git a/lib/poems/onfunctions.cpp b/lib/poems/onfunctions.cpp
new file mode 100644
index 000000000..582ab8cf8
--- /dev/null
+++ b/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);
+}
diff --git a/lib/poems/onfunctions.h b/lib/poems/onfunctions.h
new file mode 100644
index 000000000..8a6969409
--- /dev/null
+++ b/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
diff --git a/lib/poems/onsolver.cpp b/lib/poems/onsolver.cpp
new file mode 100644
index 000000000..ad04d7e9e
--- /dev/null
+++ b/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();
+ }
+}
diff --git a/lib/poems/onsolver.h b/lib/poems/onsolver.h
new file mode 100644
index 000000000..f32b5880a
--- /dev/null
+++ b/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
diff --git a/lib/poems/particle.cpp b/lib/poems/particle.cpp
new file mode 100644
index 000000000..3496bc190
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/particle.h b/lib/poems/particle.h
new file mode 100644
index 000000000..20aa4831e
--- /dev/null
+++ b/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
diff --git a/lib/poems/poemslist.h b/lib/poems/poemslist.h
new file mode 100644
index 000000000..28e345a22
--- /dev/null
+++ b/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
diff --git a/lib/poems/poemsnodelib.h b/lib/poems/poemsnodelib.h
new file mode 100644
index 000000000..e1bc4d624
--- /dev/null
+++ b/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
+
diff --git a/lib/poems/poemsobject.cpp b/lib/poems/poemsobject.cpp
new file mode 100644
index 000000000..6db3598f1
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/poemsobject.h b/lib/poems/poemsobject.h
new file mode 100644
index 000000000..d898ab3c6
--- /dev/null
+++ b/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
diff --git a/lib/poems/poemstree.h b/lib/poems/poemstree.h
new file mode 100644
index 000000000..e200ed048
--- /dev/null
+++ b/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
diff --git a/lib/poems/poemstreenode.cpp b/lib/poems/poemstreenode.cpp
new file mode 100644
index 000000000..051e13d6e
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/poemstreenode.h b/lib/poems/poemstreenode.h
new file mode 100644
index 000000000..b8f46031a
--- /dev/null
+++ b/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
+
diff --git a/lib/poems/point.cpp b/lib/poems/point.cpp
new file mode 100644
index 000000000..438384473
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/lib/poems/point.h b/lib/poems/point.h
new file mode 100644
index 000000000..746a527cd
--- /dev/null
+++ b/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
diff --git a/lib/poems/points.h b/lib/poems/points.h
new file mode 100644
index 000000000..f8b95c738
--- /dev/null
+++ b/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
diff --git a/lib/poems/prismaticjoint.cpp b/lib/poems/prismaticjoint.cpp
new file mode 100644
index 000000000..7c01667a8
--- /dev/null
+++ b/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);
+}
diff --git a/lib/poems/prismaticjoint.h b/lib/poems/prismaticjoint.h
new file mode 100644
index 000000000..17cf050a7
--- /dev/null
+++ b/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
diff --git a/lib/poems/revolutejoint.cpp b/lib/poems/revolutejoint.cpp
new file mode 100644
index 000000000..ec5b1bec0
--- /dev/null
+++ b/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);
+}
diff --git a/lib/poems/revolutejoint.h b/lib/poems/revolutejoint.h
new file mode 100644
index 000000000..478d48dbb
--- /dev/null
+++ b/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
diff --git a/lib/poems/rigidbody.cpp b/lib/poems/rigidbody.cpp
new file mode 100644
index 000000000..3ac424f9e
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/rigidbody.h b/lib/poems/rigidbody.h
new file mode 100644
index 000000000..0bb2a3ece
--- /dev/null
+++ b/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
diff --git a/lib/poems/rowmatrix.cpp b/lib/poems/rowmatrix.cpp
new file mode 100644
index 000000000..f44313765
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/rowmatrix.h b/lib/poems/rowmatrix.h
new file mode 100644
index 000000000..b04113e8e
--- /dev/null
+++ b/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
diff --git a/lib/poems/solver.cpp b/lib/poems/solver.cpp
new file mode 100644
index 000000000..67f5b607e
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/solver.h b/lib/poems/solver.h
new file mode 100644
index 000000000..45564dd1e
--- /dev/null
+++ b/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
diff --git a/lib/poems/sphericaljoint.cpp b/lib/poems/sphericaljoint.cpp
new file mode 100644
index 000000000..fb5330db1
--- /dev/null
+++ b/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);
+
+}
diff --git a/lib/poems/sphericaljoint.h b/lib/poems/sphericaljoint.h
new file mode 100644
index 000000000..ef29f8e38
--- /dev/null
+++ b/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
diff --git a/lib/poems/system.cpp b/lib/poems/system.cpp
new file mode 100644
index 000000000..25e84c2b8
--- /dev/null
+++ b/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;
+
+}
diff --git a/lib/poems/system.h b/lib/poems/system.h
new file mode 100644
index 000000000..39c1f518b
--- /dev/null
+++ b/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
diff --git a/lib/poems/vect3.cpp b/lib/poems/vect3.cpp
new file mode 100644
index 000000000..f0e677b6e
--- /dev/null
+++ b/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;
+}
diff --git a/lib/poems/vect3.h b/lib/poems/vect3.h
new file mode 100644
index 000000000..ababe18e5
--- /dev/null
+++ b/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
diff --git a/lib/poems/vect4.cpp b/lib/poems/vect4.cpp
new file mode 100644
index 000000000..4824f2883
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/vect4.h b/lib/poems/vect4.h
new file mode 100644
index 000000000..e9aee2719
--- /dev/null
+++ b/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
diff --git a/lib/poems/vect6.cpp b/lib/poems/vect6.cpp
new file mode 100644
index 000000000..a98d171d3
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/vect6.h b/lib/poems/vect6.h
new file mode 100644
index 000000000..c346c852e
--- /dev/null
+++ b/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
diff --git a/lib/poems/virtualcolmatrix.cpp b/lib/poems/virtualcolmatrix.cpp
new file mode 100644
index 000000000..05f33e793
--- /dev/null
+++ b/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);
+}
+
diff --git a/lib/poems/virtualcolmatrix.h b/lib/poems/virtualcolmatrix.h
new file mode 100644
index 000000000..1548794ca
--- /dev/null
+++ b/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
diff --git a/lib/poems/virtualmatrix.cpp b/lib/poems/virtualmatrix.cpp
new file mode 100644
index 000000000..dc9fa6049
--- /dev/null
+++ b/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;
+}
+
diff --git a/lib/poems/virtualmatrix.h b/lib/poems/virtualmatrix.h
new file mode 100644
index 000000000..a27c7de04
--- /dev/null
+++ b/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
diff --git a/lib/poems/virtualrowmatrix.cpp b/lib/poems/virtualrowmatrix.cpp
new file mode 100644
index 000000000..f57e94d23
--- /dev/null
+++ b/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);
+}
+
+
diff --git a/lib/poems/virtualrowmatrix.h b/lib/poems/virtualrowmatrix.h
new file mode 100644
index 000000000..68b39f137
--- /dev/null
+++ b/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
diff --git a/lib/poems/workspace.cpp b/lib/poems/workspace.cpp
new file mode 100644
index 000000000..54a32c124
--- /dev/null
+++ b/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();
+ }
+ }
diff --git a/lib/poems/workspace.h b/lib/poems/workspace.h
new file mode 100644
index 000000000..83af07783
--- /dev/null
+++ b/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