ICODE-ADC/lammps
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

please POEMS library in namespace POEMS and remove "using namespace" from headers

Browse Source
This commit is contained in:
Axel Kohlmeyer
2019-07-04 00:09:01 -04:00
parent 95cb995336
commit e63fe1fe84
85 changed files with 1361 additions and 1270 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
lib/poems/POEMSChain.h
View File

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

439
lib/poems/SystemProcessor.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: SystemProcessor.h * * FILE NAME: SystemProcessor.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,59 +11,61 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
#ifndef _SYS_PROCESSOR_H_ #ifndef _SYS_PROCESSOR_H_
#define _SYS_PROCESSOR_H_ #define _SYS_PROCESSOR_H_
#include <iostream>
#include "poemslist.h" #include "poemslist.h"
#include "poemstree.h" #include "poemstree.h"
#include "POEMSChain.h" #include "POEMSChain.h"
namespace POEMS {
struct POEMSNode { struct POEMSNode {
List<POEMSNode> links; List<POEMSNode> links;
List<bool> taken; List<bool> taken;
int idNumber; int idNumber;
bool visited; bool visited;
~POEMSNode(){ ~POEMSNode(){
for(int i = 0; i < taken.GetNumElements(); i++) for(int i = 0; i < taken.GetNumElements(); i++)
{ {
delete taken(i); delete taken(i);
} }
}; };
}; };
class SystemProcessor{ class SystemProcessor{
private: private:
Tree nodes; Tree nodes;
static void POEMSNodeDelete_cb(void *node) { static void POEMSNodeDelete_cb(void *node) {
delete (POEMSNode *) node; delete (POEMSNode *) node;
} }
List<POEMSChain> headsOfSystems; List<POEMSChain> headsOfSystems;
List<List<int> > ringsInSystem; List<List<int> > ringsInSystem;
POEMSNode * findSingleLink(TreeNode * aNode); POEMSNode * findSingleLink(TreeNode * aNode);
POEMSChain * AddNewChain(POEMSNode * currentNode); POEMSChain * AddNewChain(POEMSNode * currentNode);
bool setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode); bool setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode);
public: public:
SystemProcessor(void); SystemProcessor(void);
~SystemProcessor(void) { ~SystemProcessor(void) {
headsOfSystems.DeleteValues(); headsOfSystems.DeleteValues();
for(int i = 0; i < ringsInSystem.GetNumElements(); i++) for(int i = 0; i < ringsInSystem.GetNumElements(); i++)
{ {
for(int k = 0; k < ringsInSystem(i)->GetNumElements(); i++) for(int k = 0; k < ringsInSystem(i)->GetNumElements(); i++)
{ {
delete (*ringsInSystem(i))(k); delete (*ringsInSystem(i))(k);
} }
} }
}; };
void processArray(int** links, int numLinks); void processArray(int** links, int numLinks);
List<POEMSChain> * getSystemData(); List<POEMSChain> * getSystemData();
int getNumberOfHeadChains(); int getNumberOfHeadChains();
}; };
SystemProcessor::SystemProcessor(void){ SystemProcessor::SystemProcessor(void){
@ -73,145 +75,145 @@ SystemProcessor::SystemProcessor(void){
void SystemProcessor::processArray(int** links, int numLinks) 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 bool * false_var; //holds the value false; needed because a constant cannot be put into a list; the list requires a
//reference. //reference.
for(int i = 0; i < numLinks; i++) //go through all the links in the input array 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 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 POEMSNode * newNode = new POEMSNode; //make a new node
// forDeletion.Append(newNode); // forDeletion.Append(newNode);
newNode->idNumber = links[i][0]; //set its ID to the value newNode->idNumber = links[i][0]; //set its ID to the value
newNode->visited = false; //set it to be unvisited 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 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 if(!nodes.Find(links[i][1])) //repeat process for the other half of each link
{ {
POEMSNode * newNode = new POEMSNode; POEMSNode * newNode = new POEMSNode;
// forDeletion.Append(newNode); // forDeletion.Append(newNode);
newNode->idNumber = links[i][1]; newNode->idNumber = links[i][1];
newNode->visited = false; newNode->visited = false;
nodes.Insert(links[i][1], links[i][1], (void *) newNode); 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 * 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 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... firstNode->links.Append(secondNode); //and add the link from the first to the second...
false_var = new bool; false_var = new bool;
*false_var = false; //make a new false boolean to note that the link between these two *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 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 secondNode->links.Append(firstNode); //repeat process for link from second node to first
false_var = new bool; false_var = new bool;
*false_var = false; *false_var = false;
secondNode->taken.Append(false_var); secondNode->taken.Append(false_var);
} }
TreeNode * temp = nodes.GetRoot(); //get the root node of the node storage tree TreeNode * temp = nodes.GetRoot(); //get the root node of the node storage tree
POEMSNode * currentNode; POEMSNode * currentNode;
do do
{ {
currentNode = findSingleLink(temp); //find the start of the next available chain currentNode = findSingleLink(temp); //find the start of the next available chain
if(currentNode != NULL) if(currentNode != NULL)
{ {
headsOfSystems.Append(AddNewChain(currentNode)); //and add it to the headsOfSystems list of chains headsOfSystems.Append(AddNewChain(currentNode)); //and add it to the headsOfSystems list of chains
} }
} }
while(currentNode != NULL); //repeat this until all chains have been added while(currentNode != NULL); //repeat this until all chains have been added
} }
POEMSChain * SystemProcessor::AddNewChain(POEMSNode * currentNode){ POEMSChain * SystemProcessor::AddNewChain(POEMSNode * currentNode){
if(currentNode == NULL) //Termination condition; if the currentNode is null, then return null if(currentNode == NULL) //Termination condition; if the currentNode is null, then return null
{ {
return NULL; return NULL;
} }
int * tmp; 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. 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 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 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; currentNode->visited = true;
tmp = new int; tmp = new int;
*tmp = currentNode->idNumber; *tmp = currentNode->idNumber;
newChain->listOfNodes.Append(tmp); newChain->listOfNodes.Append(tmp);
return newChain; 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 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; currentNode->visited = true;
tmp = new int; tmp = new int;
*tmp = currentNode->idNumber; *tmp = currentNode->idNumber;
newChain->listOfNodes.Append(tmp); //append the current node to the chain & mark as visited newChain->listOfNodes.Append(tmp); //append the current node to the chain & mark as visited
//cout << "Appending node " << currentNode->idNumber << " to chain" << endl; //std::cout << "Appending node " << currentNode->idNumber << " to chain" << std::endl;
nextNode = currentNode->links.GetHeadElement()->value; //the next node is the first or second value stored in the links array 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... //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... 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 { //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 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); // headsOfSystems.Append(newChain);
return newChain; return newChain;
} }
nextNode = currentNode->links.GetHeadElement()->next->value;//follow the other link if there is one, so we go down the chain 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 if(!setLinkVisited(currentNode, nextNode)) //mark link as followed, so we know not to backtrack
{ {
// headsOfSystems.Append(newChain); // headsOfSystems.Append(newChain);
return newChain; //This condition, where no branches have occurred but both links have already 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 //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. //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 = nextNode; //set the current node to be the next node in the chain
} }
currentNode->visited = true; currentNode->visited = true;
tmp = new int; tmp = new int;
*tmp = currentNode->idNumber; *tmp = currentNode->idNumber;
newChain->listOfNodes.Append(tmp); //append the last node before branch (node shared jointly with branch chains) newChain->listOfNodes.Append(tmp); //append the last node before branch (node shared jointly with branch chains)
//re-mark as visited, just to make sure //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 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 POEMSChain * tempChain = NULL; //temporary variable to hold data
while(tempNode != NULL) //when we have followed all links, stop while(tempNode != NULL) //when we have followed all links, stop
{ {
if(setLinkVisited(tempNode->value, currentNode)) //dont backtrack, or create closed loops 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 = 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 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 newChain->childChains.Append(tempChain); //append the chain to this chain's list of child chains
} }
tempNode = tempNode->next; //go to process the next chain tempNode = tempNode->next; //go to process the next chain
} }
//headsOfSystems.Append(newChain); //append this chain to the system list //headsOfSystems.Append(newChain); //append this chain to the system list
return newChain; return newChain;
} }
POEMSNode * SystemProcessor::findSingleLink(TreeNode * aNode) 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 //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. //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) if(aNode == NULL)
{ {
return NULL; return NULL;
} }
POEMSNode * returnVal = (POEMSNode *)aNode->GetAuxData(); //get the poemsnode data out of the treenode 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 POEMSNode * detectLoneLoops = NULL; //is used to handle a loop that has no protruding chains
if(returnVal->visited == false) if(returnVal->visited == false)
{ {
detectLoneLoops = returnVal; //if we find any node that has not been visited yet, save it 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 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 return returnVal; //return the node is it meets this criteria
} }
returnVal = findSingleLink(aNode->Left()); //otherwise, check the left subtree returnVal = findSingleLink(aNode->Left()); //otherwise, check the left subtree
if(returnVal == NULL) //and if we find nothing... if(returnVal == NULL) //and if we find nothing...
{ {
returnVal = findSingleLink(aNode->Right()); //check the right subtree returnVal = findSingleLink(aNode->Right()); //check the right subtree
} }
if(returnVal == NULL) //if we could not find any chains if(returnVal == NULL) //if we could not find any chains
{ {
returnVal = detectLoneLoops; //see if we found any nodes at all that havent been processed 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 return returnVal; //return what we find (will be NULL if no new chains are
//found) //found)
} }
bool SystemProcessor::setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode) bool SystemProcessor::setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNode)
@ -223,65 +225,66 @@ bool SystemProcessor::setLinkVisited(POEMSNode * firstNode, POEMSNode * secondNo
//value for that particular link. Because each link is represented twice, (once at each node in the link), both of the boolean values need //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. //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 << "... "; //std::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<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 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 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(tmp->value == secondNode) //if we find the link to the other node
{ {
if(*(tmp2->value) == true) //if the link has already been visited if(*(tmp2->value) == true) //if the link has already been visited
{ {
//cout << "visited already" << endl; //std::cout << "visited already" << std::endl;
return false; //return false to indicate that the link has been visited before this attempt return false; //return false to indicate that the link has been visited before this attempt
} }
else //otherwise, visit it else //otherwise, visit it
{ {
*tmp2->value = true; *tmp2->value = true;
} }
break; break;
} }
tmp = tmp->next; //go check next link tmp = tmp->next; //go check next link
tmp2 = tmp2->next; 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 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 //it also knows this link is being visited
tmp2 = secondNode->taken.GetHeadElement(); tmp2 = secondNode->taken.GetHeadElement();
while(tmp->value != NULL || tmp2->value != NULL) //go through the list while(tmp->value != NULL || tmp2->value != NULL) //go through the list
{ {
if(tmp->value == firstNode) //if we find the link 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 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" << std::cout << "Error in parsing structure! Should never reach this condition! \n" <<
"Record of visited links out of synch between two adjacent nodes.\n"; "Record of visited links out of synch between two adjacent nodes.\n";
return false; return false;
} }
else else
{ {
*tmp2->value = true; //set the appropriate value to true to indicate this link has been visited *tmp2->value = true; //set the appropriate value to true to indicate this link has been visited
} }
break; break;
} }
tmp = tmp->next; tmp = tmp->next;
tmp2 = tmp2->next; tmp2 = tmp2->next;
} }
//cout << "not visited" << endl; //std::cout << "not visited" << std::endl;
return true; //return true to indicate that this is the first time the link has been visited 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 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 //return chains linked to the reference plane, but currently returns every chain
//in the system. //in the system.
{ {
return &headsOfSystems; return &headsOfSystems;
} }
int SystemProcessor::getNumberOfHeadChains(void) //This function isnt implemented yet, and might be taken out entirely; this was a holdover 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 //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 //It will probably be deleted once I finish figuring out exactly what needs to be returned
{ {
return 0; return 0;
}
} }
#endif #endif

5
lib/poems/body.cpp
View File

@ -26,9 +26,8 @@
#include "rigidbody.h" #include "rigidbody.h"
#include "vect3.h" #include "vect3.h"
class Joint;
using namespace std; using namespace std;
using namespace POEMS;
Body::Body() Body::Body()
{ {
@ -132,7 +131,7 @@ void Body::AddPoint(Point* point){
// global body functions // global body functions
// //
Body* NewBody(int type){ Body* POEMS::NewBody(int type){
switch( BodyType(type) ) switch( BodyType(type) )
{ {
case INERTIALFRAME : // The inertial reference frame case INERTIALFRAME : // The inertial reference frame

3
lib/poems/body.h
View File

@ -25,6 +25,7 @@
#include "mat3x3.h" #include "mat3x3.h"
#include "vect3.h" #include "vect3.h"
namespace POEMS {
// emumerated type // emumerated type
enum BodyType { enum BodyType {
INERTIALFRAME = 0, INERTIALFRAME = 0,
@ -75,5 +76,5 @@ public:
// global body functions // global body functions
Body* NewBody(int type); Body* NewBody(int type);
}
#endif #endif

3
lib/poems/body23joint.cpp
View File

@ -27,6 +27,9 @@
#include "vect3.h" #include "vect3.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std;
using namespace POEMS;
Body23Joint::Body23Joint(){ Body23Joint::Body23Joint(){
DimQandU(4,2); DimQandU(4,2);
} }

3
lib/poems/body23joint.h
View File

@ -22,6 +22,7 @@
#include "joint.h" #include "joint.h"
#include "matrix.h" #include "matrix.h"
namespace POEMS {
class Body23Joint : public Joint { class Body23Joint : public Joint {
Matrix const_sP; Matrix const_sP;
public: public:
@ -38,5 +39,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

2
lib/poems/colmatmap.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
ColMatMap::ColMatMap(){ ColMatMap::ColMatMap(){
numrows = 0; numrows = 0;

3
lib/poems/colmatmap.h
View File

@ -24,6 +24,7 @@
#include "virtualcolmatrix.h" #include "virtualcolmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class ColMatrix; class ColMatrix;
class ColMatMap : public VirtualColMatrix { class ColMatMap : public VirtualColMatrix {
@ -64,5 +65,5 @@ public:
friend void FastCKRK5(ColMatMap& X, ColMatrix& Xi, ColMatrix* f, double* c, 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); friend void FastFRK5(ColMatMap& X, ColMatrix& Xi, ColMatrix* f, double* c, double dt);
}; };
}
#endif #endif

2
lib/poems/colmatrix.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
ColMatrix::ColMatrix(){ ColMatrix::ColMatrix(){
numrows = 0; numrows = 0;

42
lib/poems/colmatrix.h
View File

@ -23,12 +23,14 @@
#include "virtualcolmatrix.h" #include "virtualcolmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Matrix; class Matrix;
class Vect6; class Vect6;
class Mat3x3; class Mat3x3;
class Vect3; class Vect3;
class ColMatrix : public VirtualColMatrix { class ColMatrix : public VirtualColMatrix {
protected:
double* elements; double* elements;
public: public:
ColMatrix(); ColMatrix();
@ -58,28 +60,28 @@ public:
ColMatrix& operator=(const VirtualMatrix& A); // overloaded = ColMatrix& operator=(const VirtualMatrix& A); // overloaded =
ColMatrix& operator*=(double b); ColMatrix& operator*=(double b);
void Abs(); void Abs();
void BasicMax(double& value, int& index); void BasicMax(double& value, int& index);
void BasicMin(double& value, int& index); void BasicMin(double& value, int& index);
// fast matrix operations // fast matrix operations
friend void FastQuaternions(ColMatrix& q, Mat3x3& C); friend void FastQuaternions(ColMatrix& q, Mat3x3& C);
friend void FastInvQuaternions(Mat3x3& C, ColMatrix& q); friend void FastInvQuaternions(Mat3x3& C, ColMatrix& q);
friend void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot); friend void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot);
friend void FastTMult(Matrix& A, Vect6& B, ColMatrix& C); friend void FastTMult(Matrix& A, Vect6& B, ColMatrix& C);
friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C); friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C);
friend void FastAssign(ColMatrix& A, ColMatrix& C); friend void FastAssign(ColMatrix& A, ColMatrix& C);
friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C); friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C); friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);
friend void FastAssign(ColMatrix&A, Vect3& C); friend void FastAssign(ColMatrix&A, Vect3& C);
friend void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot); friend void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
friend void EP_Transformation(ColMatrix& q, Mat3x3& C); friend void EP_Transformation(ColMatrix& q, Mat3x3& C);
friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C); friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C);
friend void EP_Normalize(ColMatrix& q); friend void EP_Normalize(ColMatrix& q);
friend void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot); friend void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot);
friend void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot); friend void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
}; };
}
#endif #endif

7
lib/poems/defines.h
View File

@ -15,13 +15,14 @@
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
#ifndef _DEFINES_H_ #ifndef POEMS_DEFINES_H
#define _DEFINES_H_ #define POEMS_DEFINES_H
namespace POEMS {
enum SolverType { enum SolverType {
ONSOLVER = 0, ONSOLVER = 0,
PARTICLESOLVER = 1 PARTICLESOLVER = 1
}; };
}
#endif #endif

42
lib/poems/eulerparameters.cpp
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: eulerparameters.cpp * * FILE NAME: eulerparameters.cpp *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -23,8 +23,10 @@
#include "mat3x3.h" #include "mat3x3.h"
using namespace std; using namespace std;
using namespace POEMS;
void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
void POEMS::EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
EP_Normalize(q); EP_Normalize(q);
int num=u.GetNumRows(); int num=u.GetNumRows();
if (3<num){ if (3<num){
@ -32,17 +34,17 @@ void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
qdot.elements[i]=u.elements[i-1]; 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[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[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[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]); 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){ void POEMS::EP_Transformation(ColMatrix& q, Mat3x3& C){
EP_Normalize(q); EP_Normalize(q);
double q11 = q.elements[0]*q.elements[0]; double q11 = q.elements[0]*q.elements[0];
double q22 = q.elements[1]*q.elements[1]; double q22 = q.elements[1]*q.elements[1];
double q33 = q.elements[2]*q.elements[2]; double q33 = q.elements[2]*q.elements[2];
@ -69,7 +71,7 @@ void EP_Transformation(ColMatrix& q, Mat3x3& C){
C.elements[2][1] = 2*(q23 + q14); C.elements[2][1] = 2*(q23 + q14);
} }
void EP_FromTransformation(ColMatrix& q, Mat3x3& C){ void POEMS::EP_FromTransformation(ColMatrix& q, Mat3x3& C){
double b[4]; double b[4];
// condition indicators // condition indicators
@ -117,8 +119,8 @@ void EP_FromTransformation(ColMatrix& q, Mat3x3& C){
EP_Normalize(q); EP_Normalize(q);
} }
void EP_Normalize(ColMatrix& q){ void POEMS:: 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]); 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[0] = one*q.elements[0];
q.elements[1] = one*q.elements[1]; q.elements[1] = one*q.elements[1];
q.elements[2] = one*q.elements[2]; q.elements[2] = one*q.elements[2];
@ -128,29 +130,29 @@ void EP_Normalize(ColMatrix& q){
void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq, ColMatrix& Aqddot){ void POEMS::EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq, ColMatrix& Aqddot){
int num=Audot.GetNumRows(); int num=Audot.GetNumRows();
if (3<num){ if (3<num){
for (int i=4; i<=num; i++){ for (int i=4; i<=num; i++){
Aqddot.elements[i]=Audot.elements[i-1]; Aqddot.elements[i]=Audot.elements[i-1];
} }
} }
double AA; 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]; 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[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[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[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); 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){ void POEMS::qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
EP_Normalize(q); EP_Normalize(q);
int num=qdot.GetNumRows(); int num=qdot.GetNumRows();
if (4<num){ if (4<num){
@ -161,7 +163,7 @@ void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
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[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[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]); 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]);
} }

3
lib/poems/eulerparameters.h
View File

@ -18,6 +18,7 @@
#ifndef EULERPARAMETERS_H #ifndef EULERPARAMETERS_H
#define EULERPARAMETERS_H #define EULERPARAMETERS_H
namespace POEMS {
class ColMatrix; class ColMatrix;
class Mat3x3; class Mat3x3;
@ -32,6 +33,6 @@ void EP_Normalize(ColMatrix& q);
void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot); void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq,ColMatrix& Aqddot);
void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot); void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot);
}
#endif #endif

130
lib/poems/fastmatrixops.cpp
View File

@ -29,12 +29,14 @@
#include "vect6.h" #include "vect6.h"
using namespace std; using namespace std;
using namespace POEMS;
// //
// Cross Product (friend of Vect3) // Cross Product (friend of Vect3)
// //
void FastCross(Vect3& a, Vect3& b, Vect3& c){ void POEMS::FastCross(Vect3& a, Vect3& b, Vect3& c){
c.elements[0] = a.elements[1]*b.elements[2] - a.elements[2]*b.elements[1]; 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[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]; c.elements[2] = a.elements[0]*b.elements[1] - a.elements[1]*b.elements[0];
@ -44,7 +46,7 @@ void FastCross(Vect3& a, Vect3& b, Vect3& c){
// Simple Rotation (friend of Vect3 and Mat3x3) // Simple Rotation (friend of Vect3 and Mat3x3)
// //
void FastSimpleRotation(Vect3& v, double q, Mat3x3& C){ void POEMS::FastSimpleRotation(Vect3& v, double q, Mat3x3& C){
// intermediate quantities // intermediate quantities
double cq = cos(q); double cq = cos(q);
double sq = sin(q); double sq = sin(q);
@ -69,7 +71,7 @@ void FastSimpleRotation(Vect3& v, double q, Mat3x3& C){
// Quaternion Functions // Quaternion Functions
// //
void FastQuaternions(ColMatrix& q, Mat3x3& C){ void POEMS::FastQuaternions(ColMatrix& q, Mat3x3& C){
double* e = q.elements; double* e = q.elements;
// normalize the quaternions // normalize the quaternions
@ -94,7 +96,7 @@ void FastQuaternions(ColMatrix& q, Mat3x3& C){
C.elements[2][1] = 2 * (e[2]*e[3] + e[0]*e[1]); C.elements[2][1] = 2 * (e[2]*e[3] + e[0]*e[1]);
} }
void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot){ void POEMS::FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot){
double* w = omega.elements; double* w = omega.elements;
double* e = q.elements; double* e = q.elements;
@ -104,7 +106,7 @@ void FastQuaternionDerivatives(ColMatrix& q, ColMatrix& omega, ColMatrix& qdot){
qdot.elements[3] = 0.5 * ( w[2]*e[0] + w[1]*e[1] - w[0]*e[2]); qdot.elements[3] = 0.5 * ( w[2]*e[0] + w[1]*e[1] - w[0]*e[2]);
} }
void FastInvQuaternions(Mat3x3& C, ColMatrix& q){ void POEMS::FastInvQuaternions(Mat3x3& C, ColMatrix& q){
} }
// //
@ -121,7 +123,7 @@ void FastInvQuaternions(Mat3x3& C, ColMatrix& q){
// //
// friend of Matrix // friend of Matrix
void FastLDLT(Matrix& A, Matrix& C){ // C is the LD of the LDL^T decomposition of A (SPD) void POEMS::FastLDLT(Matrix& A, Matrix& C){ // C is the LD of the LDL^T decomposition of A (SPD)
double Lv; double Lv;
int n = A.numrows; int n = A.numrows;
@ -143,7 +145,7 @@ void FastLDLT(Matrix& A, Matrix& C){ // C is the LD of the LDL^T decomposition o
// friend of Mat6x6 // friend of Mat6x6
void FastLDLT(Mat6x6& A, Mat6x6& C){ // C is the LD of the LDL^T decomposition of A (SPD) void POEMS::FastLDLT(Mat6x6& A, Mat6x6& C){ // C is the LD of the LDL^T decomposition of A (SPD)
double v[6]; double v[6];
double Lv; double Lv;
@ -165,7 +167,7 @@ void FastLDLT(Mat6x6& A, Mat6x6& C){ // C is the LD of the LDL^T decomposition o
} }
// friend of Matrix // friend of Matrix
void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C){ void POEMS::FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C){
int n = B.numrows; int n = B.numrows;
int c = B.numcols; int c = B.numcols;
double temp; double temp;
@ -190,7 +192,7 @@ void FastLDLTSubs(Matrix& LD, Matrix& B, Matrix& C){
} }
// friend of Matrix // friend of Matrix
void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C){ void POEMS::FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C){
int n = B.numcols; int n = B.numcols;
int c = B.numrows; int c = B.numrows;
double temp; double temp;
@ -215,7 +217,7 @@ void FastLDLTSubsLH(Matrix& B, Matrix& LD, Matrix& C){
} }
// friend of Mat6x6 // friend of Mat6x6
void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C){ void POEMS::FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C){
double temp; double temp;
for(int k=0;k<6;k++){ for(int k=0;k<6;k++){
@ -238,7 +240,7 @@ void FastLDLTSubs(Mat6x6& LD, Mat6x6& B, Mat6x6& C){
} }
// friend of Mat6x6 & Vect6 // friend of Mat6x6 & Vect6
void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C){ void POEMS::FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C){
double temp; double temp;
for(int i=0;i<6;i++){ for(int i=0;i<6;i++){
@ -259,7 +261,7 @@ void FastLDLTSubs(Mat6x6& LD, Vect6& B, Vect6& C){
} }
// friend of Matrix // friend of Matrix
void FastLU(Matrix& A, Matrix& LU, int *indx){ // LU is the LU decomposition of A void POEMS::FastLU(Matrix& A, Matrix& LU, int *indx){ // LU is the LU decomposition of A
int i,imax=0,j,k; int i,imax=0,j,k;
int n = A.numrows; int n = A.numrows;
double big, dum, sum, temp; double big, dum, sum, temp;
@ -309,7 +311,7 @@ void FastLU(Matrix& A, Matrix& LU, int *indx){ // LU is the LU decomposition of
} }
// friend of Mat3x3 // friend of Mat3x3
void FastLU(Mat3x3& A, Mat3x3& LU, int *indx){ // LU is the LU decomposition of A void POEMS::FastLU(Mat3x3& A, Mat3x3& LU, int *indx){ // LU is the LU decomposition of A
int i,imax=0,j,k; int i,imax=0,j,k;
double big, dum, sum, temp; double big, dum, sum, temp;
double vv[10000]; double vv[10000];
@ -357,7 +359,7 @@ void FastLU(Mat3x3& A, Mat3x3& LU, int *indx){ // LU is the LU decomposition of
} }
// friend of Mat4x4 // friend of Mat4x4
void FastLU(Mat4x4& A, Mat4x4& LU, int *indx){ // LU is the LU decomposition of A void POEMS::FastLU(Mat4x4& A, Mat4x4& LU, int *indx){ // LU is the LU decomposition of A
int i,imax=0,j,k; int i,imax=0,j,k;
double big, dum, sum, temp; double big, dum, sum, temp;
double vv[10000]; double vv[10000];
@ -405,7 +407,7 @@ void FastLU(Mat4x4& A, Mat4x4& LU, int *indx){ // LU is the LU decomposition of
} }
// friend of Mat6x6 // friend of Mat6x6
void FastLU(Mat6x6& A, Mat6x6& LU, int *indx){ // LU is the LU decomposition of A void POEMS::FastLU(Mat6x6& A, Mat6x6& LU, int *indx){ // LU is the LU decomposition of A
int i,imax=0,j,k; int i,imax=0,j,k;
double big, dum, sum, temp; double big, dum, sum, temp;
double vv[10000]; double vv[10000];
@ -453,7 +455,7 @@ void FastLU(Mat6x6& A, Mat6x6& LU, int *indx){ // LU is the LU decomposition of
} }
// friend of Matrix // friend of Matrix
void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution void POEMS::FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
int i,ip,j,k; int i,ip,j,k;
int n = B.numrows; int n = B.numrows;
int c = B.numcols; int c = B.numcols;
@ -477,7 +479,7 @@ void FastLUSubs(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate For
} }
// friend of Matrix and Mat3x3 // friend of Matrix and Mat3x3
void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution void POEMS::FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
int i,ip,j,k; int i,ip,j,k;
int n = B.numrows; int n = B.numrows;
int c = B.numcols; int c = B.numcols;
@ -501,7 +503,7 @@ void FastLUSubs(Mat3x3& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate For
} }
// friend of Matrix and Mat4x4 // friend of Matrix and Mat4x4
void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution void POEMS::FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
int i,ip,j,k; int i,ip,j,k;
int n = B.numrows; int n = B.numrows;
int c = B.numcols; int c = B.numcols;
@ -525,7 +527,7 @@ void FastLUSubs(Mat4x4& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate For
} }
// friend of Matrix and Mat6x6 // friend of Matrix and Mat6x6
void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution void POEMS::FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
int i,ip,j,k; int i,ip,j,k;
int n = B.numrows; int n = B.numrows;
int c = B.numcols; int c = B.numcols;
@ -551,7 +553,7 @@ void FastLUSubs(Mat6x6& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate For
// The following LUSubsLH routine is incomplete at the moment. // The following LUSubsLH routine is incomplete at the moment.
// friend of Matrix // friend of Matrix
void FastLUSubsLH(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution void POEMS::FastLUSubsLH(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate Forward and Back Substitution
int i,ip,j,k; int i,ip,j,k;
int n = B.numcols; int n = B.numcols;
int c = B.numrows; int c = B.numrows;
@ -582,13 +584,13 @@ void FastLUSubsLH(Matrix& LU, Matrix& B, Matrix& C, int *indx){ // Appropriate F
// Triple sum // Triple sum
// //
void FastTripleSum(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b+c void POEMS::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[0] = a.elements[0]+b.elements[0]+c.elements[0];
d.elements[1] = a.elements[1]+b.elements[1]+c.elements[1]; d.elements[1] = a.elements[1]+b.elements[1]+c.elements[1];
d.elements[2] = a.elements[2]+b.elements[2]+c.elements[2]; 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 void POEMS::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[0] = a.elements[0]+b.elements[0]-c.elements[0];
d.elements[1] = a.elements[1]+b.elements[1]-c.elements[1]; d.elements[1] = a.elements[1]+b.elements[1]-c.elements[1];
d.elements[2] = a.elements[2]+b.elements[2]-c.elements[2]; d.elements[2] = a.elements[2]+b.elements[2]-c.elements[2];
@ -599,7 +601,7 @@ void FastTripleSumPPM(Vect3& a, Vect3& b, Vect3& c, Vect3& d){ // d = a+b-c
// //
// friend of matrix // friend of matrix
void FastMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B void POEMS::FastMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
// assumes dimensions are already correct! // assumes dimensions are already correct!
int r = A.numrows; int r = A.numrows;
int ca = A.numcols; int ca = A.numcols;
@ -615,7 +617,7 @@ void FastMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
} }
// friend of matrix // friend of matrix
void FastTMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B void POEMS::FastTMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
// assumes dimensions are already correct! // assumes dimensions are already correct!
int r = A.numcols; int r = A.numcols;
int ca = A.numrows; int ca = A.numrows;
@ -631,14 +633,14 @@ void FastTMult(Matrix& A, Matrix& B, Matrix& C){ // C = A*B
} }
// friend of Mat3x3 & Vect3 // friend of Mat3x3 & Vect3
void FastMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = A*B void POEMS::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[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[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]; 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 // friend of Mat3x3, ColMatrix, & Vect3
void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C){ // C = A*B void POEMS::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[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[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]; C.elements[2] = A.elements[2][0]*B.elements[0] + A.elements[2][1]*B.elements[1] + A.elements[2][2]*B.elements[2];
@ -646,42 +648,42 @@ void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C){ // C = A*B
// friend of Mat3x3, ColMatrix, & Vect3 // friend of Mat3x3, ColMatrix, & Vect3
void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C){ // C = A*B void POEMS::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[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[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]; 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 // friend of Mat3x3 & Vect3
void FastTMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = A^T*B void POEMS::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[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[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]; 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 // friend of Mat3x3 & Vect3
void FastNegMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = -A*B void POEMS::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[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[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]; 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 // friend of Mat3x3 & Vect3
void FastNegTMult(Mat3x3& A, Vect3& B, Vect3& C){ // C = -A^T*B void POEMS::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[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[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]; 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 // friend of Vect3
void FastMult(double a, Vect3& B, Vect3& C){ // C = a*B void POEMS::FastMult(double a, Vect3& B, Vect3& C){ // C = a*B
C.elements[0] = a*B.elements[0]; C.elements[0] = a*B.elements[0];
C.elements[1] = a*B.elements[1]; C.elements[1] = a*B.elements[1];
C.elements[2] = a*B.elements[2]; C.elements[2] = a*B.elements[2];
} }
// friend of Mat4x4 & Vect4 // friend of Mat4x4 & Vect4
void FastMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A*B void POEMS::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[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[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[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];
@ -689,7 +691,7 @@ void FastMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A*B
} }
// friend of Mat4x4 & Vect4 // friend of Mat4x4 & Vect4
void FastTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A^T*B void POEMS::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[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[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[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];
@ -697,7 +699,7 @@ void FastTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = A^T*B
} }
// friend of Mat4x4 & Vect4 // friend of Mat4x4 & Vect4
void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A*B void POEMS::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[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[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[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];
@ -705,7 +707,7 @@ void FastNegMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A*B
} }
// friend of Mat4x4 & Vect4 // friend of Mat4x4 & Vect4
void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A^T*B void POEMS::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[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[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[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];
@ -713,7 +715,7 @@ void FastNegTMult(Mat4x4& A, Vect4& B, Vect4& C){ // C = -A^T*B
} }
// friend of Vect4 // friend of Vect4
void FastMult(double a, Vect4& B, Vect4& C){ // C = a*B void POEMS::FastMult(double a, Vect4& B, Vect4& C){ // C = a*B
C.elements[0] = a*B.elements[0]; C.elements[0] = a*B.elements[0];
C.elements[1] = a*B.elements[1]; C.elements[1] = a*B.elements[1];
C.elements[2] = a*B.elements[2]; C.elements[2] = a*B.elements[2];
@ -721,7 +723,7 @@ void FastMult(double a, Vect4& B, Vect4& C){ // C = a*B
} }
// friend of Matrix & Mat6x6 // friend of Matrix & Mat6x6
void FastMultT(Matrix& A, Matrix& B, Mat6x6& C){ // C = A*B^T void POEMS::FastMultT(Matrix& A, Matrix& B, Mat6x6& C){ // C = A*B^T
int i,j,k,n; int i,j,k,n;
n = A.numcols; n = A.numcols;
@ -734,7 +736,7 @@ void FastMultT(Matrix& A, Matrix& B, Mat6x6& C){ // C = A*B^T
} }
// friend Matrix, Vect6, ColMatrix // friend Matrix, Vect6, ColMatrix
void FastMult(Matrix& A, ColMatrix& B, Vect6& C){ void POEMS::FastMult(Matrix& A, ColMatrix& B, Vect6& C){
int ca = A.numcols; int ca = A.numcols;
int i,k; int i,k;
@ -746,7 +748,7 @@ void FastMult(Matrix& A, ColMatrix& B, Vect6& C){
} }
// friend of Matrix & Mat6x6 // friend of Matrix & Mat6x6
void FastMult(Mat6x6& A, Matrix& B, Matrix& C){ // C = A*B void POEMS::FastMult(Mat6x6& A, Matrix& B, Matrix& C){ // C = A*B
// assumes dimensions are already correct! // assumes dimensions are already correct!
int cb = B.numcols; int cb = B.numcols;
@ -760,7 +762,7 @@ void FastMult(Mat6x6& A, Matrix& B, Matrix& C){ // C = A*B
} }
// friend Matrix, Vect6, ColMatrix // friend Matrix, Vect6, ColMatrix
void FastTMult(Matrix& A, Vect6& B, ColMatrix& C){ // C = A^T*B void POEMS::FastTMult(Matrix& A, Vect6& B, ColMatrix& C){ // C = A^T*B
int n = C.numrows; int n = C.numrows;
int i,k; int i,k;
for(i=0;i<n;i++){ for(i=0;i<n;i++){
@ -771,7 +773,7 @@ void FastTMult(Matrix& A, Vect6& B, ColMatrix& C){ // C = A^T*B
} }
// friend of Mat3x3 // friend of Mat3x3
void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B void POEMS::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][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][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[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];
@ -786,7 +788,7 @@ void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B
} }
// friend of Mat3x3 // friend of Mat3x3
void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B^T void POEMS::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][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][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[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];
@ -801,7 +803,7 @@ void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C){ // C = A*B^T
} }
// friend of Mat4x4 // friend of Mat4x4
void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B void POEMS::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][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][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][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];
@ -824,7 +826,7 @@ void FastMult(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B
} }
// friend of Mat4x4 // friend of Mat4x4
void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B^T void POEMS::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][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][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][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];
@ -848,7 +850,7 @@ void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C){ // C = A*B^T
} }
// friend of Mat6x6 // friend of Mat6x6
void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B void POEMS::FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
int i,j,k; int i,j,k;
for(i=0;i<6;i++) for(i=0;i<6;i++)
for(j=0;j<6;j++){ for(j=0;j<6;j++){
@ -859,7 +861,7 @@ void FastMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
} }
// friend of Mat6x6 // friend of Mat6x6
void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B void POEMS::FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
int i,j,k; int i,j,k;
for(i=0;i<6;i++) for(i=0;i<6;i++)
for(j=0;j<6;j++){ for(j=0;j<6;j++){
@ -870,7 +872,7 @@ void FastMultT(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A*B
} }
// friend of Mat6x6 // friend of Mat6x6
void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A^T*B void POEMS::FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A^T*B
int i,j,k; int i,j,k;
for(i=0;i<6;i++) for(i=0;i<6;i++)
for(j=0;j<6;j++){ for(j=0;j<6;j++){
@ -881,13 +883,13 @@ void FastTMult(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A^T*B
} }
// friend of Mat6x6 & Vect6 // friend of Mat6x6 & Vect6
void FastMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A*B void POEMS::FastMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A*B
for(int i=0;i<6;i++) 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]; 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 // friend of Mat6x6 & Vect6
void FastTMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A^T*B void POEMS::FastTMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A^T*B
for(int i=0;i<6;i++) 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]; 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];
} }
@ -897,21 +899,21 @@ void FastTMult(Mat6x6& A, Vect6& B, Vect6& C){ // C = A^T*B
// //
// friend of Vect3 // friend of Vect3
void FastAdd(Vect3& A, Vect3& B, Vect3& C){ // C = A+B void POEMS::FastAdd(Vect3& A, Vect3& B, Vect3& C){ // C = A+B
C.elements[0] = A.elements[0] + B.elements[0]; C.elements[0] = A.elements[0] + B.elements[0];
C.elements[1] = A.elements[1] + B.elements[1]; C.elements[1] = A.elements[1] + B.elements[1];
C.elements[2] = A.elements[2] + B.elements[2]; C.elements[2] = A.elements[2] + B.elements[2];
} }
// friend of Vect4 // friend of Vect4
void FastAdd(Vect4& A, Vect4& B, Vect4& C){ // C = A+B void POEMS::FastAdd(Vect4& A, Vect4& B, Vect4& C){ // C = A+B
C.elements[0] = A.elements[0] + B.elements[0]; C.elements[0] = A.elements[0] + B.elements[0];
C.elements[1] = A.elements[1] + B.elements[1]; C.elements[1] = A.elements[1] + B.elements[1];
C.elements[2] = A.elements[2] + B.elements[2]; C.elements[2] = A.elements[2] + B.elements[2];
C.elements[3] = A.elements[3] + B.elements[3]; C.elements[3] = A.elements[3] + B.elements[3];
} }
void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A+B void POEMS::FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A+B
int i,j; int i,j;
for(i=0;i<6;i++) for(i=0;i<6;i++)
for(j=0;j<6;j++) for(j=0;j<6;j++)
@ -919,7 +921,7 @@ void FastAdd(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A+B
} }
// friend of Vect6 // friend of Vect6
void FastAdd(Vect6& A, Vect6& B, Vect6& C){ // C = A-B void POEMS::FastAdd(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
C.elements[0] = A.elements[0] + B.elements[0]; C.elements[0] = A.elements[0] + B.elements[0];
C.elements[1] = A.elements[1] + B.elements[1]; C.elements[1] = A.elements[1] + B.elements[1];
C.elements[2] = A.elements[2] + B.elements[2]; C.elements[2] = A.elements[2] + B.elements[2];
@ -933,21 +935,21 @@ void FastAdd(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
// //
// friend of Vect3 // friend of Vect3
void FastSubt(Vect3& A, Vect3& B, Vect3& C){ // C = A-B void POEMS::FastSubt(Vect3& A, Vect3& B, Vect3& C){ // C = A-B
C.elements[0] = A.elements[0] - B.elements[0]; C.elements[0] = A.elements[0] - B.elements[0];
C.elements[1] = A.elements[1] - B.elements[1]; C.elements[1] = A.elements[1] - B.elements[1];
C.elements[2] = A.elements[2] - B.elements[2]; C.elements[2] = A.elements[2] - B.elements[2];
} }
// friend of Vect4 // friend of Vect4
void FastSubt(Vect4& A, Vect4& B, Vect4& C){ // C = A-B void POEMS::FastSubt(Vect4& A, Vect4& B, Vect4& C){ // C = A-B
C.elements[0] = A.elements[0] - B.elements[0]; C.elements[0] = A.elements[0] - B.elements[0];
C.elements[1] = A.elements[1] - B.elements[1]; C.elements[1] = A.elements[1] - B.elements[1];
C.elements[2] = A.elements[2] - B.elements[2]; C.elements[2] = A.elements[2] - B.elements[2];
C.elements[3] = A.elements[3] - B.elements[3]; C.elements[3] = A.elements[3] - B.elements[3];
} }
void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A-B void POEMS::FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A-B
int i,j; int i,j;
for(i=0;i<6;i++) for(i=0;i<6;i++)
for(j=0;j<6;j++) for(j=0;j<6;j++)
@ -955,7 +957,7 @@ void FastSubt(Mat6x6& A, Mat6x6& B, Mat6x6& C){ // C = A-B
} }
// friend of Vect6 // friend of Vect6
void FastSubt(Vect6& A, Vect6& B, Vect6& C){ // C = A-B void POEMS::FastSubt(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
C.elements[0] = A.elements[0] - B.elements[0]; C.elements[0] = A.elements[0] - B.elements[0];
C.elements[1] = A.elements[1] - B.elements[1]; C.elements[1] = A.elements[1] - B.elements[1];
C.elements[2] = A.elements[2] - B.elements[2]; C.elements[2] = A.elements[2] - B.elements[2];
@ -965,33 +967,33 @@ void FastSubt(Vect6& A, Vect6& B, Vect6& C){ // C = A-B
} }
// friend of ColMatMap // friend of ColMatMap
void FastAssign(ColMatMap& A, ColMatMap& C){ void POEMS::FastAssign(ColMatMap& A, ColMatMap& C){
for(int i=0;i<C.numrows;i++) for(int i=0;i<C.numrows;i++)
*(C.elements[i]) = *(A.elements[i]); *(C.elements[i]) = *(A.elements[i]);
} }
// friend of ColMatrix // friend of ColMatrix
void FastAssign(ColMatrix& A, ColMatrix& C){ //C = A void POEMS::FastAssign(ColMatrix& A, ColMatrix& C){ //C = A
for(int i=0;i<C.numrows;i++) for(int i=0;i<C.numrows;i++)
C.elements[i] = A.elements[i]; C.elements[i] = A.elements[i];
} }
// friend of Vect3 // friend of Vect3
void FastAssign(Vect3& A, Vect3& C){ //C = A void POEMS::FastAssign(Vect3& A, Vect3& C){ //C = A
C.elements[0] = A.elements[0]; C.elements[0] = A.elements[0];
C.elements[1] = A.elements[1]; C.elements[1] = A.elements[1];
C.elements[2] = A.elements[2]; C.elements[2] = A.elements[2];
} }
// friend of Vect3 & ColMatrix // friend of Vect3 & ColMatrix
void FastAssign(ColMatrix& A, Vect3& C){ //C = A void POEMS::FastAssign(ColMatrix& A, Vect3& C){ //C = A
C.elements[0] = A.elements[0]; C.elements[0] = A.elements[0];
C.elements[1] = A.elements[1]; C.elements[1] = A.elements[1];
C.elements[2] = A.elements[2]; C.elements[2] = A.elements[2];
} }
// friend of Vect4 // friend of Vect4
void FastAssign(Vect4& A, Vect4& C){ //C = A void POEMS::FastAssign(Vect4& A, Vect4& C){ //C = A
C.elements[0] = A.elements[0]; C.elements[0] = A.elements[0];
C.elements[1] = A.elements[1]; C.elements[1] = A.elements[1];
C.elements[2] = A.elements[2]; C.elements[2] = A.elements[2];
@ -999,7 +1001,7 @@ void FastAssign(Vect4& A, Vect4& C){ //C = A
} }
// friend of Mat3x3 // friend of Mat3x3
void FastAssignT(Mat3x3& A, Mat3x3& C){ void POEMS::FastAssignT(Mat3x3& A, Mat3x3& C){
C.elements[0][0] = A.elements[0][0]; C.elements[0][0] = A.elements[0][0];
C.elements[1][1] = A.elements[1][1]; C.elements[1][1] = A.elements[1][1];
C.elements[2][2] = A.elements[2][2]; C.elements[2][2] = A.elements[2][2];
@ -1015,7 +1017,7 @@ void FastAssignT(Mat3x3& A, Mat3x3& C){
} }
// friend of Mat4x4 // friend of Mat4x4
void FastAssignT(Mat4x4& A, Mat4x4& C){ void POEMS::FastAssignT(Mat4x4& A, Mat4x4& C){
C.elements[0][0] = A.elements[0][0]; C.elements[0][0] = A.elements[0][0];
C.elements[1][1] = A.elements[1][1]; C.elements[1][1] = A.elements[1][1];
C.elements[2][2] = A.elements[2][2]; C.elements[2][2] = A.elements[2][2];

3
lib/poems/fastmatrixops.h
View File

@ -18,6 +18,7 @@
#ifndef FASTMATRIXOPS_H #ifndef FASTMATRIXOPS_H
#define FASTMATRIXOPS_H #define FASTMATRIXOPS_H
namespace POEMS {
class ColMatMap; class ColMatMap;
class ColMatrix; class ColMatrix;
class Mat3x3; class Mat3x3;
@ -102,5 +103,5 @@ void FastAssign(ColMatrix&A, Vect3& C);
void FastAssign(Vect4& A, Vect4& C); // C = A void FastAssign(Vect4& A, Vect4& C); // C = A
void FastAssignT(Mat3x3& A, Mat3x3& C); // C = A^T void FastAssignT(Mat3x3& A, Mat3x3& C); // C = A^T
void FastAssignT(Mat4x4& A, Mat4x4& C); // C = A^T void FastAssignT(Mat4x4& A, Mat4x4& C); // C = A^T
}
#endif #endif

2
lib/poems/fixedpoint.cpp
View File

@ -24,6 +24,8 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
FixedPoint::FixedPoint(){ FixedPoint::FixedPoint(){
} }

3
lib/poems/fixedpoint.h
View File

@ -24,6 +24,7 @@
#include "point.h" #include "point.h"
#include "vect3.h" #include "vect3.h"
namespace POEMS {
class FixedPoint : public Point { class FixedPoint : public Point {
public: public:
FixedPoint(); FixedPoint();
@ -35,5 +36,5 @@ public:
bool ReadInPointData(std::istream& in); bool ReadInPointData(std::istream& in);
void WriteOutPointData(std::ostream& out); void WriteOutPointData(std::ostream& out);
}; };
}
#endif #endif

126
lib/poems/freebodyjoint.cpp
View File

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

8
lib/poems/freebodyjoint.h
View File

@ -23,12 +23,12 @@
#include "joint.h" #include "joint.h"
#include "matrix.h" #include "matrix.h"
namespace POEMS {
class FreeBodyJoint : public Joint{ class FreeBodyJoint : public Joint{
public: public:
FreeBodyJoint(); FreeBodyJoint();
~FreeBodyJoint(); ~FreeBodyJoint();
JointType GetType(); JointType GetType();
bool ReadInJointData(std::istream& in); bool ReadInJointData(std::istream& in);
void WriteOutJointData(std::ostream& out); void WriteOutJointData(std::ostream& out);
@ -40,5 +40,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

2
lib/poems/inertialframe.cpp
View File

@ -21,6 +21,8 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
InertialFrame::InertialFrame(){ InertialFrame::InertialFrame(){
gravity.Zeros(); gravity.Zeros();

4
lib/poems/inertialframe.h
View File

@ -24,7 +24,7 @@
#include "body.h" #include "body.h"
#include "vect3.h" #include "vect3.h"
namespace POEMS {
class InertialFrame : public Body { class InertialFrame : public Body {
Vect3 gravity; Vect3 gravity;
public: public:
@ -36,5 +36,5 @@ public:
bool ReadInBodyData(std::istream& in); bool ReadInBodyData(std::istream& in);
void WriteOutBodyData(std::ostream& out); void WriteOutBodyData(std::ostream& out);
}; };
}
#endif #endif

4
lib/poems/joint.cpp
View File

@ -36,6 +36,8 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
Joint::Joint(){ Joint::Joint(){
body1 = body2 = 0; body1 = body2 = 0;
@ -245,7 +247,7 @@ void Joint::ComputeBackwardGlobalTransform(){
// global joint functions // global joint functions
// //
Joint* NewJoint(int type){ Joint* POEMS::NewJoint(int type){
switch( JointType(type) ) switch( JointType(type) )
{ {
case FREEBODYJOINT : return new FreeBodyJoint; case FREEBODYJOINT : return new FreeBodyJoint;

3
lib/poems/joint.h
View File

@ -25,6 +25,7 @@
#include "matrix.h" #include "matrix.h"
#include "vect3.h" #include "vect3.h"
namespace POEMS {
class VirtualMatrix; class VirtualMatrix;
enum JointType { enum JointType {
@ -124,5 +125,5 @@ public:
// global joint functions // global joint functions
Joint* NewJoint(int type); Joint* NewJoint(int type);
}
#endif #endif

2
lib/poems/mat3x3.cpp
View File

@ -21,6 +21,8 @@
#include "mat3x3.h" #include "mat3x3.h"
using namespace std; using namespace std;
using namespace POEMS;
Mat3x3::Mat3x3(){ Mat3x3::Mat3x3(){
numrows = numcols = 3; numrows = numcols = 3;

14
lib/poems/mat3x3.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: mat3x3.h * * FILE NAME: mat3x3.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -21,6 +21,7 @@
#include <iostream> #include <iostream>
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Vect3; class Vect3;
class Mat6x6; class Mat6x6;
class ColMatrix; class ColMatrix;
@ -67,16 +68,15 @@ public:
friend void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C); friend void FastMult(Mat3x3& A, Mat3x3& B, Mat3x3& C);
friend void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C); friend void FastMultT(Mat3x3& A, Mat3x3& B, Mat3x3& C);
friend void FastAssignT(Mat3x3& A, Mat3x3& C); friend void FastAssignT(Mat3x3& A, Mat3x3& C);
friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C); friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);
friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC); friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI); friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C); friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
friend void EP_Transformation(ColMatrix& q, Mat3x3& C); friend void EP_Transformation(ColMatrix& q, Mat3x3& C);
friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C); friend void EP_FromTransformation(ColMatrix& q, Mat3x3& C);
}; };
}
#endif #endif

2
lib/poems/mat4x4.cpp
View File

@ -21,6 +21,8 @@
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace POEMS;
Mat4x4::Mat4x4(){ Mat4x4::Mat4x4(){
numrows = numcols = 4; numrows = numcols = 4;

11
lib/poems/mat4x4.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: mat4x4.h * * FILE NAME: mat4x4.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -21,17 +21,18 @@
#include <iostream> #include <iostream>
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Vect4; class Vect4;
class Matrix; class Matrix;
class Mat4x4 : public VirtualMatrix { class Mat4x4 : public VirtualMatrix {
double elements[4][4]; double elements[4][4];
public: public:
Mat4x4(); Mat4x4();
~Mat4x4(); ~Mat4x4();
Mat4x4(const Mat4x4& A); // copy constructor Mat4x4(const Mat4x4& A); // copy constructor
Mat4x4(const VirtualMatrix& A); // copy constructor Mat4x4(const VirtualMatrix& A); // copy constructor
double& operator_2int (int i, int j); // array access double& operator_2int (int i, int j); // array access
double Get_2int(int i, int j) const; double Get_2int(int i, int j) const;
void Set_2int(int i, int j, double value); void Set_2int(int i, int j, double value);
@ -64,5 +65,5 @@ public:
friend void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C); friend void FastMultT(Mat4x4& A, Mat4x4& B, Mat4x4& C);
friend void FastAssignT(Mat4x4& A, Mat4x4& C); friend void FastAssignT(Mat4x4& A, Mat4x4& C);
}; };
}
#endif #endif

2
lib/poems/mat6x6.cpp
View File

@ -21,6 +21,8 @@
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace POEMS;
Mat6x6::Mat6x6(){ Mat6x6::Mat6x6(){
numrows = numcols = 6; numrows = numcols = 6;

3
lib/poems/mat6x6.h
View File

@ -21,6 +21,7 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Matrix; class Matrix;
class Mat3x3; class Mat3x3;
class Vect6; class Vect6;
@ -73,5 +74,5 @@ public:
friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC); friend void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC);
friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI); friend void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
}; };
}
#endif #endif

13
lib/poems/matrices.h
View File

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

2
lib/poems/matrix.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
Matrix::Matrix(){ Matrix::Matrix(){
numrows = numcols = 0; numrows = numcols = 0;

8
lib/poems/matrix.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: matrix.h * * FILE NAME: matrix.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -22,6 +22,7 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Mat3x3; class Mat3x3;
class Mat4x4; class Mat4x4;
class Mat6x6; class Mat6x6;
@ -73,7 +74,6 @@ public:
friend void FastMult(Mat6x6& A, Matrix& B, Matrix& C); friend void FastMult(Mat6x6& A, Matrix& B, Matrix& C);
friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C); friend void FastMult(Matrix& A, ColMatrix& B, Vect6& C);
friend void FastMultT(Matrix& A, Matrix& B, Mat6x6& C); friend void FastMultT(Matrix& A, Matrix& B, Mat6x6& C);
}; };
}
#endif #endif

48
lib/poems/matrixfun.cpp
View File

@ -27,12 +27,14 @@
#include "virtualrowmatrix.h" #include "virtualrowmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
// //
// Create a new matrix // Create a new matrix
// //
VirtualMatrix* NewMatrix(int type){ VirtualMatrix* POEMS::NewMatrix(int type){
switch( MatrixType(type) ) switch( MatrixType(type) )
{ {
case MATRIX : return new Matrix; case MATRIX : return new Matrix;
@ -50,7 +52,7 @@ VirtualMatrix* NewMatrix(int type){
// Transpose // Transpose
// //
Matrix T(const VirtualMatrix& A){ Matrix POEMS::T(const VirtualMatrix& A){
int numrows = A.GetNumRows(); int numrows = A.GetNumRows();
int numcols = A.GetNumCols(); int numcols = A.GetNumCols();
Matrix C(numcols,numrows); Matrix C(numcols,numrows);
@ -60,8 +62,8 @@ Matrix T(const VirtualMatrix& A){
return C; return C;
} }
Mat3x3 T(const Mat3x3& A){ Mat3x3 POEMS::T(const Mat3x3& A){
Mat3x3 C; Mat3x3 C;
C.elements[0][0] = A.elements[0][0]; C.elements[0][0] = A.elements[0][0];
C.elements[1][1] = A.elements[1][1]; C.elements[1][1] = A.elements[1][1];
C.elements[2][2] = A.elements[2][2]; C.elements[2][2] = A.elements[2][2];
@ -76,7 +78,7 @@ Mat3x3 T(const Mat3x3& A){
return C; return C;
} }
Mat6x6 T(const Mat6x6& A){ Mat6x6 POEMS::T(const Mat6x6& A){
Mat6x6 C; Mat6x6 C;
int i,j; int i,j;
for(i=0;i<6;i++) for(i=0;i<6;i++)
@ -86,7 +88,7 @@ Mat6x6 T(const Mat6x6& A){
return C; return C;
} }
Matrix T(const Vect3& A){ Matrix POEMS::T(const Vect3& A){
Matrix C(1,3); Matrix C(1,3);
C.BasicSet(0,0,A.elements[0]); C.BasicSet(0,0,A.elements[0]);
C.BasicSet(0,1,A.elements[1]); C.BasicSet(0,1,A.elements[1]);
@ -95,7 +97,7 @@ Matrix T(const Vect3& A){
return C; return C;
} }
Matrix T(const Vect6& A){ Matrix POEMS::T(const Vect6& A){
Matrix C(1,6); Matrix C(1,6);
C.BasicSet(0,0,A.elements[0]); C.BasicSet(0,0,A.elements[0]);
C.BasicSet(0,1,A.elements[1]); C.BasicSet(0,1,A.elements[1]);
@ -107,7 +109,7 @@ Matrix T(const Vect6& A){
return C; return C;
} }
RowMatrix T(const VirtualColMatrix &A){ RowMatrix POEMS::T(const VirtualColMatrix &A){
int numele = A.GetNumRows(); int numele = A.GetNumRows();
RowMatrix C(numele); RowMatrix C(numele);
for(int i=0;i<numele;i++) for(int i=0;i<numele;i++)
@ -115,7 +117,7 @@ RowMatrix T(const VirtualColMatrix &A){
return C; return C;
} }
ColMatrix T(const VirtualRowMatrix &A){ ColMatrix POEMS::T(const VirtualRowMatrix &A){
int numele = A.GetNumCols(); int numele = A.GetNumCols();
ColMatrix C(numele); ColMatrix C(numele);
for(int i=0;i<numele;i++) for(int i=0;i<numele;i++)
@ -127,7 +129,7 @@ ColMatrix T(const VirtualRowMatrix &A){
// Symmetric Inverse // Symmetric Inverse
// //
Matrix SymInverse(Matrix &A){ Matrix POEMS::SymInverse(Matrix &A){
int r = A.GetNumRows(); int r = A.GetNumRows();
Matrix C(r,r); Matrix C(r,r);
Matrix LD(r,r); Matrix LD(r,r);
@ -139,7 +141,7 @@ Matrix SymInverse(Matrix &A){
return C; return C;
} }
Mat6x6 SymInverse(Mat6x6 &A){ Mat6x6 POEMS::SymInverse(Mat6x6 &A){
Mat6x6 C; Mat6x6 C;
Mat6x6 LD; Mat6x6 LD;
Mat6x6 I; Mat6x6 I;
@ -207,7 +209,7 @@ Mat6x6 Inverse(Mat6x6& A){
// overloaded addition // overloaded addition
// //
Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B){ // addition Matrix POEMS::operator+ (const VirtualMatrix &A, const VirtualMatrix &B){ // addition
int Arows,Acols,Brows,Bcols; int Arows,Acols,Brows,Bcols;
Arows = A.GetNumRows(); Arows = A.GetNumRows();
Acols = A.GetNumCols(); Acols = A.GetNumCols();
@ -232,7 +234,7 @@ Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B){ // addit
// overloaded subtraction // overloaded subtraction
// //
Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B){ // subtraction Matrix POEMS::operator- (const VirtualMatrix &A, const VirtualMatrix &B){ // subtraction
int Arows,Acols,Brows,Bcols; int Arows,Acols,Brows,Bcols;
Arows = A.GetNumRows(); Arows = A.GetNumRows();
Acols = A.GetNumCols(); Acols = A.GetNumCols();
@ -257,7 +259,7 @@ Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B){ // subtr
// overloaded matrix multiplication // overloaded matrix multiplication
// //
Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B){ // multiplication Matrix POEMS::operator* (const VirtualMatrix &A, const VirtualMatrix &B){ // multiplication
int Arows,Acols,Brows,Bcols; int Arows,Acols,Brows,Bcols;
Arows = A.GetNumRows(); Arows = A.GetNumRows();
Acols = A.GetNumCols(); Acols = A.GetNumCols();
@ -284,13 +286,13 @@ Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B){ // multi
// overloaded scalar multiplication // overloaded scalar multiplication
// //
Matrix operator* (const VirtualMatrix &A, double b){ // multiplication Matrix POEMS::operator* (const VirtualMatrix &A, double b){ // multiplication
Matrix C = A; Matrix C = A;
C *= b; C *= b;
return C; return C;
} }
Matrix operator* (double b, const VirtualMatrix &A){ // multiplication Matrix POEMS::operator* (double b, const VirtualMatrix &A){ // multiplication
Matrix C = A; Matrix C = A;
C *= b; C *= b;
return C; return C;
@ -300,7 +302,7 @@ Matrix operator* (double b, const VirtualMatrix &A){ // multiplication
// overloaded negative // overloaded negative
// //
Matrix operator- (const VirtualMatrix& A){ // negative Matrix POEMS::operator- (const VirtualMatrix& A){ // negative
int r = A.GetNumRows(); int r = A.GetNumRows();
int c = A.GetNumCols(); int c = A.GetNumCols();
Matrix C(r,c); Matrix C(r,c);
@ -316,7 +318,7 @@ Matrix operator- (const VirtualMatrix& A){ // negative
// Cross product (friend of Vect3) // Cross product (friend of Vect3)
// //
Vect3 Cross(Vect3& a, Vect3& b){ Vect3 POEMS::Cross(Vect3& a, Vect3& b){
return CrossMat(a)*b; return CrossMat(a)*b;
} }
@ -324,7 +326,7 @@ Vect3 Cross(Vect3& a, Vect3& b){
// Cross Matrix (friend of Vect3 & Mat3x3) // Cross Matrix (friend of Vect3 & Mat3x3)
// //
Mat3x3 CrossMat(Vect3& a){ Mat3x3 POEMS::CrossMat(Vect3& a){
Mat3x3 C; Mat3x3 C;
C.Zeros(); C.Zeros();
C.elements[0][1] = -a.elements[2]; C.elements[0][1] = -a.elements[2];
@ -341,7 +343,7 @@ Mat3x3 CrossMat(Vect3& a){
// Stack // Stack
// //
Matrix Stack(VirtualMatrix& A, VirtualMatrix& B){ Matrix POEMS::Stack(VirtualMatrix& A, VirtualMatrix& B){
int m,na,nb; int m,na,nb;
m = A.GetNumCols(); m = A.GetNumCols();
if( m != B.GetNumCols()){ if( m != B.GetNumCols()){
@ -370,7 +372,7 @@ Matrix Stack(VirtualMatrix& A, VirtualMatrix& B){
//Hstack //Hstack
Matrix HStack(VirtualMatrix& A, VirtualMatrix& B){ Matrix POEMS::HStack(VirtualMatrix& A, VirtualMatrix& B){
int m,na,nb; int m,na,nb;
m = A.GetNumRows(); m = A.GetNumRows();
if( m != B.GetNumRows()){ if( m != B.GetNumRows()){
@ -400,13 +402,13 @@ Matrix HStack(VirtualMatrix& A, VirtualMatrix& B){
// //
// //
void Set6DAngularVector(Vect6& v6, Vect3& v3){ void POEMS::Set6DAngularVector(Vect6& v6, Vect3& v3){
v6.elements[0] = v3.elements[0]; v6.elements[0] = v3.elements[0];
v6.elements[1] = v3.elements[1]; v6.elements[1] = v3.elements[1];
v6.elements[2] = v3.elements[2]; v6.elements[2] = v3.elements[2];
} }
void Set6DLinearVector(Vect6& v6, Vect3& v3){ void POEMS::Set6DLinearVector(Vect6& v6, Vect3& v3){
v6.elements[3] = v3.elements[0]; v6.elements[3] = v3.elements[0];
v6.elements[4] = v3.elements[1]; v6.elements[4] = v3.elements[1];
v6.elements[5] = v3.elements[2]; v6.elements[5] = v3.elements[2];

17
lib/poems/matrixfun.h
View File

@ -27,6 +27,7 @@
#include "vect3.h" #include "vect3.h"
#include "vect6.h" #include "vect6.h"
namespace POEMS {
class VirtualColMatrix; class VirtualColMatrix;
class VirtualMatrix; class VirtualMatrix;
class VirtualRowMatrix; class VirtualRowMatrix;
@ -55,22 +56,22 @@ Mat4x4 Inverse(Mat4x4& A);
Mat6x6 Inverse(Mat6x6& A); Mat6x6 Inverse(Mat6x6& A);
// overloaded addition // overloaded addition
Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B); // addition Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B); // addition
//Mat3x3 operator+ (const Mat3x3 &A, const Mat3x3 &B); // addition //Mat3x3 operator+ (const Mat3x3 &A, const Mat3x3 &B); // addition
//Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B); // addition //Matrix operator+ (const VirtualMatrix &A, const VirtualMatrix &B); // addition
// overloaded subtraction // overloaded subtraction
Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B); // subtraction Matrix operator- (const VirtualMatrix &A, const VirtualMatrix &B); // subtraction
// overloaded matrix multiplication // overloaded matrix multiplication
Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B); // multiplication Matrix operator* (const VirtualMatrix &A, const VirtualMatrix &B); // multiplication
// overloaded scalar-matrix multiplication // overloaded scalar-matrix multiplication
Matrix operator* (const VirtualMatrix &A, double b); // overloaded * Matrix operator* (const VirtualMatrix &A, double b); // overloaded *
Matrix operator* (double b, const VirtualMatrix &A); // overloaded * Matrix operator* (double b, const VirtualMatrix &A); // overloaded *
// overloaded negative // overloaded negative
Matrix operator- (const VirtualMatrix &A); // negative Matrix operator- (const VirtualMatrix &A); // negative
Vect3 Cross(Vect3& a, Vect3& b); Vect3 Cross(Vect3& a, Vect3& b);
Mat3x3 CrossMat(Vect3& a); Mat3x3 CrossMat(Vect3& a);
@ -80,5 +81,5 @@ Matrix HStack(VirtualMatrix& A, VirtualMatrix& B);
void Set6DAngularVector(Vect6& v6, Vect3& v3); void Set6DAngularVector(Vect6& v6, Vect3& v3);
void Set6DLinearVector(Vect6& v6, Vect3& v3); void Set6DLinearVector(Vect6& v6, Vect3& v3);
}
#endif #endif

130
lib/poems/mixedjoint.cpp
View File

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

12
lib/poems/mixedjoint.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: mixedjoint.h * * FILE NAME: mixedjoint.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -24,16 +24,16 @@
#include "matrix.h" #include "matrix.h"
#include "vect6.h" #include "vect6.h"
namespace POEMS {
class MixedJoint : public Joint{ class MixedJoint : public Joint{
Matrix const_sP; Matrix const_sP;
int numrots; int numrots;
int numtrans; int numtrans;
Vect6 dofs; Vect6 dofs;
public: public:
MixedJoint(); MixedJoint();
~MixedJoint(); ~MixedJoint();
JointType GetType(); JointType GetType();
bool ReadInJointData(std::istream& in); bool ReadInJointData(std::istream& in);
void WriteOutJointData(std::ostream& out); void WriteOutJointData(std::ostream& out);
@ -46,5 +46,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

2
lib/poems/norm.cpp
View File

@ -24,6 +24,8 @@
#include "vect4.h" #include "vect4.h"
#include "vect6.h" #include "vect6.h"
using namespace POEMS;
double Magnitude(ColMatrix& A){ double Magnitude(ColMatrix& A){
double G; double G;
G = 0; G = 0;

7
lib/poems/norm.h
View File

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

2
lib/poems/onbody.cpp
View File

@ -30,6 +30,8 @@
#include "colmatrix.h" #include "colmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
OnBody::OnBody(){ OnBody::OnBody(){

106
lib/poems/onbody.h
View File

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

40
lib/poems/onfunctions.cpp
View File

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

3
lib/poems/onfunctions.h
View File

@ -18,6 +18,7 @@
#ifndef ONFUNCTIONS_H #ifndef ONFUNCTIONS_H
#define ONFUNCTIONS_H #define ONFUNCTIONS_H
namespace POEMS {
class Mat3x3; class Mat3x3;
class Mat6x6; class Mat6x6;
class Vect3; class Vect3;
@ -30,5 +31,5 @@ void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI);
void Create_Map(int MM); void Create_Map(int MM);
int ICELL(int IX,int IY,int IZ, int MM); int ICELL(int IX,int IY,int IZ, int MM);
}
#endif #endif

2
lib/poems/onsolver.cpp
View File

@ -33,6 +33,8 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
OnSolver::OnSolver(){ OnSolver::OnSolver(){
numbodies = 0; numbodies = 0;

49
lib/poems/onsolver.h
View File

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

2
lib/poems/particle.cpp
View File

@ -18,6 +18,8 @@
#include "particle.h" #include "particle.h"
using namespace POEMS;
Particle::Particle(){ Particle::Particle(){
} }

10
lib/poems/particle.h
View File

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

74
lib/poems/poemslist.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: poemslist.h * * FILE NAME: poemslist.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -21,8 +21,7 @@
#include <iostream> #include <iostream>
#include <cstdlib> #include <cstdlib>
using namespace std; namespace POEMS {
template<class T> class ListElement{ template<class T> class ListElement{
public: public:
ListElement<T>* prev; ListElement<T>* prev;
@ -51,7 +50,7 @@ public:
S* operator()(int id); S* operator()(int id);
void Append(List<S> * listToAppend); void Append(List<S> * listToAppend);
void DeleteValues(); void DeleteValues();
void RemoveElementAndDeleteValue(ListElement<S>* ele); void RemoveElementAndDeleteValue(ListElement<S>* ele);
void PrintList(); void PrintList();
}; };
@ -90,9 +89,9 @@ template<class S> List<S>::~List(){
template<class S> void List<S>::Append(List<S> * listToAppend) template<class S> void List<S>::Append(List<S> * listToAppend)
{ {
tail->next = listToAppend->head; tail->next = listToAppend->head;
listToAppend->head->prev = tail; listToAppend->head->prev = tail;
tail = listToAppend->tail; tail = listToAppend->tail;
} }
template<class S> int List<S>::GetNumElements(){ template<class S> int List<S>::GetNumElements(){
@ -104,17 +103,17 @@ template<class S> ListElement<S>* List<S>::GetHeadElement(){
} }
template<class S> ListElement<S>* List<S>::GetTailElement(){ template<class S> ListElement<S>* List<S>::GetTailElement(){
return tail; return tail;
} }
template<class S> void List<S>::Remove(ListElement<S>* ele){ template<class S> void List<S>::Remove(ListElement<S>* ele){
if(!ele){ if(!ele){
cerr << "ERROR: ListElement to be removed not defined" << endl; std::cerr << "ERROR: ListElement to be removed not defined" << std::endl;
exit(0); exit(0);
} }
if(!numelements){ if(!numelements){
cerr << "ERROR: List is empty" << endl; std::cerr << "ERROR: List is empty" << std::endl;
exit(0); exit(0);
} }
@ -135,7 +134,7 @@ template<class S> void List<S>::Remove(ListElement<S>* ele){
template<class S> ListElement<S>* List<S>::Append(S* v){ template<class S> ListElement<S>* List<S>::Append(S* v){
if(!v){ if(!v){
cerr << "ERROR: cannot add null Body to list" << endl; std::cerr << "ERROR: cannot add null Body to list" << std::endl;
exit(0); exit(0);
} }
@ -145,22 +144,22 @@ template<class S> ListElement<S>* List<S>::Append(S* v){
if(numelements==1) if(numelements==1)
head = tail = ele; head = tail = ele;
else{ else{
/* /*
tail->next = ele; tail->next = ele;
ele->prev = tail; ele->prev = tail;
tail = ele;*/ tail = ele;*/
ele->prev = tail; ele->prev = tail;
tail = ele; tail = ele;
ele->prev->next = ele; ele->prev->next = ele;
} }
return ele; return ele;
} }
template<class S> ListElement<S>* List<S>::Prepend(S* v){ template<class S> ListElement<S>* List<S>::Prepend(S* v){
if(!v){ if(!v){
cerr << "ERROR: cannot add null Body to list" << endl; std::cerr << "ERROR: cannot add null Body to list" << std::endl;
exit(0); exit(0);
} }
@ -170,9 +169,9 @@ template<class S> ListElement<S>* List<S>::Prepend(S* v){
if(numelements==1) if(numelements==1)
head = tail = ele; head = tail = ele;
else{ else{
ele->next = head; ele->next = head;
head = ele; head = ele;
ele->next->prev = ele; ele->next->prev = ele;
} }
return ele; return ele;
} }
@ -190,15 +189,15 @@ template<class S> S** List<S>::CreateArray(){
template<class S> S* List<S>::operator()(int id){ template<class S> S* List<S>::operator()(int id){
if(id >= numelements){ if(id >= numelements){
cerr << "ERROR: subscript out of bounds" << endl; std::cerr << "ERROR: subscript out of bounds" << std::endl;
exit(0); exit(0);
} }
ListElement<S>* ele = head; ListElement<S>* ele = head;
for(int i=0;i<id;i++){ for(int i=0;i<id;i++){
ele = ele->next; ele = ele->next;
} }
return ele->value; return ele->value;
} }
@ -214,16 +213,15 @@ template<class S> void List<S>::RemoveElementAndDeleteValue(ListElement<S>* ele)
} }
template<class S> void List<S>::PrintList(){ template<class S> void List<S>::PrintList(){
cout<<"Printing List "<<endl; std::cout << "Printing List " << std::endl;
ListElement<S>* ele = head; ListElement<S>* ele = head;
cout<<*(ele->value)<<" "; std::cout << *(ele->value) << " ";
ele = ele->next; ele = ele->next;
for(int k =2; k<numelements; k++){ for(int k =2; k<numelements; k++){
cout<<*(ele->value)<<" "; std::cout << *(ele->value) << " ";
ele = ele->next; ele = ele->next;
} }
cout<<*(ele->value)<<endl; std::cout << *(ele->value) << std::endl;
}
} }
#endif #endif

11
lib/poems/poemsnodelib.h
View File

@ -20,9 +20,7 @@
#include <iostream> #include <iostream>
using namespace std; namespace POEMS {
TreeNode *GetTreeNode(int item,TreeNode *lptr = NULL,TreeNode *rptr =NULL); TreeNode *GetTreeNode(int item,TreeNode *lptr = NULL,TreeNode *rptr =NULL);
void FreeTreeNode(TreeNode *p); void FreeTreeNode(TreeNode *p);
@ -81,7 +79,7 @@ TreeNode *GetTreeNode(int item,TreeNode *lptr,TreeNode *rptr)
// if insufficient memory, terminatewith an error message // if insufficient memory, terminatewith an error message
if (p == NULL) if (p == NULL)
{ {
cerr << "Memory allocation failure!\n"; std::cerr << "Memory allocation failure!\n";
exit(1); exit(1);
} }
@ -140,7 +138,7 @@ void IndentBlanks(int num)
// const int indentblock = 6; // const int indentblock = 6;
for(int i = 0; i < num; i++) for(int i = 0; i < num; i++)
cout << " "; std::cout << " ";
} }
void PrintTree (TreeNode *t, int level) void PrintTree (TreeNode *t, int level)
@ -153,10 +151,11 @@ void PrintTree (TreeNode *t, int level)
PrintTree(t->Right(),level + 1); PrintTree(t->Right(),level + 1);
// indent to current level; output node data // indent to current level; output node data
IndentBlanks(indentUnit*level); IndentBlanks(indentUnit*level);
cout << t->GetData() << endl; std::cout << t->GetData() << std::endl;
// print left branch of tree t // print left branch of tree t
PrintTree(t->Left(),level + 1); PrintTree(t->Left(),level + 1);
} }
} }
}
#endif #endif

2
lib/poems/poemsobject.cpp
View File

@ -19,6 +19,8 @@
#include "poemsobject.h" #include "poemsobject.h"
#include <cstring> #include <cstring>
using namespace POEMS;
POEMSObject::POEMSObject(){ POEMSObject::POEMSObject(){
name = 0; name = 0;
ChangeName((const char*)"unnamed"); ChangeName((const char*)"unnamed");

4
lib/poems/poemsobject.h
View File

@ -19,7 +19,7 @@
#ifndef POEMSOBJECT_H #ifndef POEMSOBJECT_H
#define POEMSOBJECT_H #define POEMSOBJECT_H
namespace POEMS {
class POEMSObject { class POEMSObject {
char* name; char* name;
int ID; int ID;
@ -31,5 +31,5 @@ public:
int GetID(); int GetID();
void SetID(int id); void SetID(int id);
}; };
}
#endif #endif

4
lib/poems/poemstree.h
View File

@ -21,7 +21,7 @@
#include "poemstreenode.h" #include "poemstreenode.h"
#include "poemsnodelib.h" #include "poemsnodelib.h"
namespace POEMS {
// constants to indicate the balance factor of a node // constants to indicate the balance factor of a node
const int leftheavy = -1; const int leftheavy = -1;
const int balanced = 0; const int balanced = 0;
@ -609,5 +609,5 @@ void Tree::ClearList(void)
delete current; delete current;
size = 0; size = 0;
} }
}
#endif #endif

2
lib/poems/poemstreenode.cpp
View File

@ -17,6 +17,8 @@
#include "poemstreenode.h" #include "poemstreenode.h"
using namespace POEMS;
// constructor; initialize the data and pointer fields // constructor; initialize the data and pointer fields
// The pointer value NULL assigns a empty subtree // The pointer value NULL assigns a empty subtree
TreeNode::TreeNode (const int & item, TreeNode *lptr,TreeNode *rptr, TreeNode::TreeNode (const int & item, TreeNode *lptr,TreeNode *rptr,

39
lib/poems/poemstreenode.h
View File

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

4
lib/poems/point.cpp
View File

@ -20,6 +20,8 @@
#include "point.h" #include "point.h"
#include "vect3.h" #include "vect3.h"
using namespace POEMS;
Point::Point(){ Point::Point(){
position.Zeros(); position.Zeros();
} }
@ -35,7 +37,7 @@ void Point::WriteOut(std::ostream& out){
WriteOutPointData(out); WriteOutPointData(out);
} }
Point* NewPoint(int type){ Point* POEMS::NewPoint(int type){
switch( PointType(type) ) switch( PointType(type) )
{ {
case FIXEDPOINT : // A Fixed Point case FIXEDPOINT : // A Fixed Point

18
lib/poems/point.h
View File

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

2
lib/poems/prismaticjoint.cpp
View File

@ -25,6 +25,8 @@
#include "mat3x3.h" #include "mat3x3.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace POEMS;
PrismaticJoint::PrismaticJoint(){ PrismaticJoint::PrismaticJoint(){
q.Dim(1); q.Dim(1);
qdot.Dim(1); qdot.Dim(1);

5
lib/poems/prismaticjoint.h
View File

@ -24,8 +24,7 @@
#include "vect3.h" #include "vect3.h"
#include "matrix.h" #include "matrix.h"
namespace POEMS {
class PrismaticJoint : public Joint { class PrismaticJoint : public Joint {
Vect3 axis_pk; // unit vector in body1 basis Vect3 axis_pk; // unit vector in body1 basis
Vect3 axis_k; // unit vector in body2 basis Vect3 axis_k; // unit vector in body2 basis
@ -45,5 +44,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

2
lib/poems/revolutejoint.cpp
View File

@ -25,6 +25,8 @@
#include "mat3x3.h" #include "mat3x3.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace POEMS;
RevoluteJoint::RevoluteJoint(){ RevoluteJoint::RevoluteJoint(){
DimQandU(1); DimQandU(1);
Vect3 axis; Vect3 axis;

3
lib/poems/revolutejoint.h
View File

@ -23,6 +23,7 @@
#include "vect3.h" #include "vect3.h"
#include "matrix.h" #include "matrix.h"
namespace POEMS {
class VirtualMatrix; class VirtualMatrix;
class RevoluteJoint : public Joint { class RevoluteJoint : public Joint {
@ -44,5 +45,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

2
lib/poems/rigidbody.cpp
View File

@ -20,6 +20,8 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std; using namespace std;
using namespace POEMS;
RigidBody::RigidBody(){ RigidBody::RigidBody(){
} }

3
lib/poems/rigidbody.h
View File

@ -22,6 +22,7 @@
#include "body.h" #include "body.h"
namespace POEMS {
class RigidBody : public Body { class RigidBody : public Body {
public: public:
RigidBody(); RigidBody();
@ -30,5 +31,5 @@ public:
bool ReadInBodyData(std::istream& in); bool ReadInBodyData(std::istream& in);
void WriteOutBodyData(std::ostream& out); void WriteOutBodyData(std::ostream& out);
}; };
}
#endif #endif

2
lib/poems/rowmatrix.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
RowMatrix::RowMatrix(){ RowMatrix::RowMatrix(){
numcols = 0; numcols = 0;

3
lib/poems/rowmatrix.h
View File

@ -24,6 +24,7 @@
#include "virtualrowmatrix.h" #include "virtualrowmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class RowMatrix : public VirtualRowMatrix { class RowMatrix : public VirtualRowMatrix {
double* elements; double* elements;
public: public:
@ -52,5 +53,5 @@ public:
RowMatrix& operator=(const VirtualMatrix& A); // overloaded = RowMatrix& operator=(const VirtualMatrix& A); // overloaded =
RowMatrix& operator*=(double b); RowMatrix& operator*=(double b);
}; };
}
#endif #endif

2
lib/poems/solver.cpp
View File

@ -20,6 +20,8 @@
#include "system.h" #include "system.h"
#include "onsolver.h" #include "onsolver.h"
using namespace POEMS;
Solver::Solver(){ Solver::Solver(){
} }

19
lib/poems/solver.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: solver.h * * FILE NAME: solver.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -21,6 +21,7 @@
#include "colmatmap.h" #include "colmatmap.h"
#include "defines.h" #include "defines.h"
namespace POEMS {
class System; class System;
class Matrix; class Matrix;
@ -28,11 +29,11 @@ class Solver{
protected: protected:
System* system; System* system;
double time; double time;
ColMatMap state; ColMatMap state;
ColMatMap statedot; ColMatMap statedot;
ColMatMap statedoubledot; ColMatMap statedoubledot;
SolverType type; SolverType type;
@ -47,13 +48,13 @@ public:
virtual void DeleteModel() = 0; virtual void DeleteModel() = 0;
virtual void CreateModel() = 0; virtual void CreateModel() = 0;
virtual void Solve(double time, Matrix& FF) = 0; virtual void Solve(double time, Matrix& FF) = 0;
ColMatMap* GetState(); ColMatMap* GetState();
ColMatMap* GetStateDerivative(); ColMatMap* GetStateDerivative();
ColMatMap* GetStateDerivativeDerivative(); ColMatMap* GetStateDerivativeDerivative();
}; };
}
#endif #endif

190
lib/poems/sphericaljoint.cpp
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: sphericaljoint.cpp * * FILE NAME: sphericaljoint.cpp *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,10 +11,10 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
#include "sphericaljoint.h" #include "sphericaljoint.h"
@ -30,6 +30,8 @@
#include "vect3.h" #include "vect3.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
using namespace std;
using namespace POEMS;
SphericalJoint::SphericalJoint(){ SphericalJoint::SphericalJoint(){
DimQandU(4,3); DimQandU(4,3);
@ -48,24 +50,24 @@ bool SphericalJoint::ReadInJointData(std::istream& in){
void SphericalJoint::WriteOutJointData(std::ostream& out){ void SphericalJoint::WriteOutJointData(std::ostream& out){
} }
Matrix SphericalJoint::GetForward_sP(){ Matrix SphericalJoint::GetForward_sP(){
Mat3x3 sPa,sPl; Mat3x3 sPa,sPl;
Matrix sP(6,3); Matrix sP(6,3);
sPa.Identity(); sPa.Identity();
sPl.Zeros(); sPl.Zeros();
Vect3 temp = -(point2->position); Vect3 temp = -(point2->position);
sPl(1,2) = temp(3); sPl(1,2) = temp(3);
sPl(1,3) = -temp(2); sPl(1,3) = -temp(2);
sPl(2,1) = -temp(3); sPl(2,1) = -temp(3);
sPl(2,3) = temp(1); sPl(2,3) = temp(1);
sPl(3,1) = temp(2); sPl(3,1) = temp(2);
sPl(3,2) = -temp(1); sPl(3,2) = -temp(1);
sP=Stack(sPa,sPl); sP=Stack(sPa,sPl);
return sP; return sP;
} }
void SphericalJoint::UpdateForward_sP( Matrix& sP){ void SphericalJoint::UpdateForward_sP( Matrix& sP){
@ -79,15 +81,15 @@ Matrix SphericalJoint::GetBackward_sP(){
cout<<" -----------"<<endl; cout<<" -----------"<<endl;
Mat3x3 sPa,sPl; Mat3x3 sPa,sPl;
Matrix sP; Matrix sP;
sPa.Identity(); sPa.Identity();
sPl.Zeros(); sPl.Zeros();
sPl(3,2)=(point2->position(1)); sPl(3,2)=(point2->position(1));
sPl(2,3)=-(point2->position(1)); sPl(2,3)=-(point2->position(1));
sP=Stack(sPa,sPl); sP=Stack(sPa,sPl);
return sP; return sP;
} }
void SphericalJoint::UpdateBackward_sP( Matrix& sP){ void SphericalJoint::UpdateBackward_sP( Matrix& sP){
// sP is constant, do nothing. // sP is constant, do nothing.
} }
@ -96,106 +98,106 @@ void SphericalJoint::ComputeLocalTransform(){
// Obtain the transformation matrix from euler parameters // Obtain the transformation matrix from euler parameters
EP_Transformation(q, ko_C_k); EP_Transformation(q, ko_C_k);
FastMult(pk_C_ko,ko_C_k,pk_C_k); FastMult(pk_C_ko,ko_C_k,pk_C_k);
} }
void SphericalJoint::ForwardKinematics(){ void SphericalJoint::ForwardKinematics(){
Vect3 result1,result2,result3,result4,result5; Vect3 result1,result2,result3,result4,result5;
Vect3 pk_w_k; Vect3 pk_w_k;
//cout<<"Check in spherical "<<q<<" "<<qdot<<endl; //cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
EP_Normalize(q); EP_Normalize(q);
// orientations // orientations
ComputeForwardTransforms(); ComputeForwardTransforms();
//----------------------------------// //----------------------------------//
// COMPUTE POSITION VECTOR R12 aka GAMMA // COMPUTE POSITION VECTOR R12 aka GAMMA
FastNegMult(pk_C_k,point2->position,result1); // parents basis FastNegMult(pk_C_k,point2->position,result1); // parents basis
FastAdd(result1,point1->position,r12); FastAdd(result1,point1->position,r12);
// compute position vector r21 // compute position vector r21
FastNegMult(k_C_pk,r12,r21); FastNegMult(k_C_pk,r12,r21);
//----------------------------------// //----------------------------------//
// COMPUTE GLOBAL LOCATION // COMPUTE GLOBAL LOCATION
FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1); FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);
FastAdd(result1,body1->r,result1); FastAdd(result1,body1->r,result1);
FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2); FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);
FastAdd(result1,result2,body2->r); FastAdd(result1,result2,body2->r);
qdot_to_u(q, u, qdot); qdot_to_u(q, u, qdot);
//-----------------------------------
// angular velocities
//-----------------------------------
// angular velocities
FastAssign(u,pk_w_k); FastAssign(u,pk_w_k);
FastTMult(pk_C_k,body1->omega_k,result1); FastTMult(pk_C_k,body1->omega_k,result1);
FastAdd(result1,pk_w_k,body2->omega_k); FastAdd(result1,pk_w_k,body2->omega_k);
FastMult(body2->n_C_k,body2->omega_k,body2->omega); FastMult(body2->n_C_k,body2->omega_k,body2->omega);
//----------------------------------- //-----------------------------------
// compute velocities // compute velocities
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1); FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
FastAdd(body1->v_k,result1,result2); FastAdd(body1->v_k,result1,result2);
FastTMult(pk_C_k,result2,result1); // In body basis FastTMult(pk_C_k,result2,result1); // In body basis
FastCross((body2->GetPoint(1))->position,body2->omega_k,result2); FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);
FastAdd(result1,result2,body2->v_k); // In body basis FastAdd(result1,result2,body2->v_k); // In body basis
FastMult(body2->n_C_k,body2->v_k,body2->v); FastMult(body2->n_C_k,body2->v_k,body2->v);
//------------------------------------------ //------------------------------------------
//Compute the KE //Compute the KE
Matrix tempke; Matrix tempke;
tempke = T(body2->v)*(body2->v); tempke = T(body2->v)*(body2->v);
double ke = 0.0; double ke = 0.0;
ke = body2->mass*tempke(1,1); ke = body2->mass*tempke(1,1);
FastMult(body2->inertia,body2->omega_k,result1); FastMult(body2->inertia,body2->omega_k,result1);
tempke= T(body2->omega_k)*result1; tempke= T(body2->omega_k)*result1;
ke = 0.5*ke + 0.5*tempke(1,1); ke = 0.5*ke + 0.5*tempke(1,1);
body2->KE=ke; body2->KE=ke;
//----------------------------------- //-----------------------------------
// compute state explicit angular acceleration // Has to be in body basis // compute state explicit angular acceleration // Has to be in body basis
FastTMult(pk_C_k,body1->alpha_t,result2); FastTMult(pk_C_k,body1->alpha_t,result2);
FastCross(body2->omega_k,pk_w_k,result1); FastCross(body2->omega_k,pk_w_k,result1);
FastAdd(result1,result2,body2->alpha_t); FastAdd(result1,result2,body2->alpha_t);
//----------------------------------- //-----------------------------------
// compute state explicit acceleration // compute state explicit acceleration
// NEED TO DO THIS COMPLETELY IN BODY BASIS // NEED TO DO THIS COMPLETELY IN BODY BASIS
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1); FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
FastCross(body1->omega_k,result1,result2); FastCross(body1->omega_k,result1,result2);
FastTMult(pk_C_k,result2,result1); FastTMult(pk_C_k,result2,result1);
//FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3); //FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);
FastCross((body2->GetPoint(1))->position,body2->omega_k,result3); FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);
FastCross(body2->omega_k,result3,result2); FastCross(body2->omega_k,result3,result2);
FastAdd(result1,result2,result3); //wxwxr in body basis FastAdd(result1,result2,result3); //wxwxr in body basis
FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4); FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4);
FastTMult(pk_C_k,result4,result5); FastTMult(pk_C_k,result4,result5);
FastAssign(result5,result4); FastAssign(result5,result4);
FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2); FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);
FastAdd(result2,result4,result1); //alphaxr in body basis FastAdd(result2,result4,result1); //alphaxr in body basis
FastTMult(pk_C_k,body1->a_t,result2); FastTMult(pk_C_k,body1->a_t,result2);
FastTripleSum(result3,result1,result2,body2->a_t); // in body basis FastTripleSum(result3,result1,result2,body2->a_t); // in body basis
//----------------------------------- //-----------------------------------
} }
// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT // NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
@ -206,13 +208,13 @@ void SphericalJoint::BackwardKinematics(){
// orientations // orientations
ComputeBackwardTransforms(); ComputeBackwardTransforms();
// compute position vector r21 // compute position vector r21
//r21 = point2->position - k_C_pk * point1->position; //r21 = point2->position - k_C_pk * point1->position;
FastMult(k_C_pk,point1->position,result1); FastMult(k_C_pk,point1->position,result1);
FastSubt(point2->position,result1,r21); FastSubt(point2->position,result1,r21);
// compute position vector r21 // compute position vector r21
FastNegMult(pk_C_k,r21,r12); FastNegMult(pk_C_k,r21,r12);
@ -231,7 +233,7 @@ void SphericalJoint::BackwardKinematics(){
EP_Derivatives(q,u,qdot); EP_Derivatives(q,u,qdot);
// angular velocities // angular velocities
FastMult(body2->n_C_k,u,result2); FastMult(body2->n_C_k,u,result2);
FastAdd(body2->omega,result2,body1->omega); FastAdd(body2->omega,result2,body1->omega);
FastAssign(u,k_w_pk); FastAssign(u,k_w_pk);
@ -251,7 +253,7 @@ void SphericalJoint::BackwardKinematics(){
FastCross(body1->omega_k,k_w_pk,result1); FastCross(body1->omega_k,k_w_pk,result1);
FastMult(pk_C_k,body2->alpha_t,result2); FastMult(pk_C_k,body2->alpha_t,result2);
FastAdd(result1,result2,body1->alpha_t); FastAdd(result1,result2,body1->alpha_t);
// compute state explicit acceleration // compute state explicit acceleration
FastCross(body2->alpha_t,point2->position,result1); FastCross(body2->alpha_t,point2->position,result1);
FastCross(body2->omega_k,point2->position,result2); FastCross(body2->omega_k,point2->position,result2);
@ -264,5 +266,5 @@ void SphericalJoint::BackwardKinematics(){
FastCross(body1->omega_k,result2,result3); FastCross(body1->omega_k,result2,result3);
FastTripleSum(result5,result1,result3,body1->a_t); FastTripleSum(result5,result1,result3,body1->a_t);
} }

3
lib/poems/sphericaljoint.h
View File

@ -22,6 +22,7 @@
#include "joint.h" #include "joint.h"
#include "matrix.h" #include "matrix.h"
namespace POEMS {
class SphericalJoint : public Joint { class SphericalJoint : public Joint {
Matrix const_sP; Matrix const_sP;
public: public:
@ -38,5 +39,5 @@ public:
void ForwardKinematics(); void ForwardKinematics();
void BackwardKinematics(); void BackwardKinematics();
}; };
}
#endif #endif

615
lib/poems/system.cpp
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: system.cpp * * FILE NAME: system.cpp *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -36,11 +36,12 @@
#include "vect3.h" #include "vect3.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
class Point; namespace POEMS { class Point; }
using namespace std;
using namespace POEMS;
System::System(){ System::System(){
mappings = NULL; mappings = NULL;
} }
System::~System(){ System::~System(){
@ -59,7 +60,7 @@ int System::GetNumBodies(){
int * System::GetMappings() int * System::GetMappings()
{ {
return mappings; return mappings;
} }
void System::AddBody(Body* body){ void System::AddBody(Body* body){
@ -78,16 +79,16 @@ double System::GetTime(){
return time; return time;
} }
void System::ComputeForces(){ void System::ComputeForces(){
// NOT DOING ANYTHING AT THIS TIME // NOT DOING ANYTHING AT THIS TIME
} }
bool System::ReadIn(istream& in){ bool System::ReadIn(istream& in){
int numbodies; int numbodies;
Body* body; Body* body;
int bodytype; int bodytype;
char bodyname[256]; char bodyname[256];
int index; int index;
// get number of bodies // get number of bodies
in >> numbodies; in >> numbodies;
@ -165,12 +166,12 @@ bool System::ReadIn(istream& in){
delete [] bodyarray; delete [] bodyarray;
return false; return false;
} }
joint->SetBodies(bodyarray[body1], bodyarray[body2]); joint->SetBodies(bodyarray[body1], bodyarray[body2]);
bodyarray[body1]->AddJoint(joint); bodyarray[body1]->AddJoint(joint);
bodyarray[body2]->AddJoint(joint); bodyarray[body2]->AddJoint(joint);
in >> point1 >> point2; in >> point1 >> point2;
joint->SetPoints(bodyarray[body1]->GetPoint(point1),bodyarray[body2]->GetPoint(point2)); joint->SetPoints(bodyarray[body1]->GetPoint(point1),bodyarray[body2]->GetPoint(point2));
@ -203,7 +204,7 @@ void System::WriteOut(ostream& out){
// set the body ID for later identification // set the body ID for later identification
body->SetID(i); body->SetID(i);
// write out the data // write out the data
body->WriteOut(out); body->WriteOut(out);
@ -211,7 +212,7 @@ void System::WriteOut(ostream& out){
} }
// number of joints // number of joints
out << joints.GetNumElements() << endl; out << joints.GetNumElements() << endl;
// joints loop // joints loop
i = 0; i = 0;
@ -226,7 +227,7 @@ void System::WriteOut(ostream& out){
// write out the data // write out the data
joint->WriteOut(out); joint->WriteOut(out);
i++; j_ele = j_ele->next; i++; j_ele = j_ele->next;
} }
} }
@ -252,68 +253,68 @@ void System::ClearJointIDs(){
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){ 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 // Declaring Temporary Entities
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
Body* body = NULL; Body* body = NULL;
Body* prev; Body* prev;
Body* Inertial; Body* Inertial;
Point* origin; Point* origin;
Joint* joint; Joint* joint;
Point* point_CM; Point* point_CM;
Point* point_p; Point* point_p;
Point* point_k; Point* point_k;
Point* point_ch = NULL; Point* point_ch = NULL;
Vect3 r1,r2,r3,v1,v2,v3; Vect3 r1,r2,r3,v1,v2,v3;
Mat3x3 IM, N, PKCK,PKCN ; Mat3x3 IM, N, PKCK,PKCN ;
ColMatrix qo, uo, q, qdot,w; ColMatrix qo, uo, q, qdot,w;
mappings = new int[nfree]; mappings = new int[nfree];
for(int i = 0; i < nfree; i++) for(int i = 0; i < nfree; i++)
{ {
mappings[i] = freelist[i]; mappings[i] = freelist[i];
} }
qo.Dim(4); qo.Dim(4);
uo.Dim(3); uo.Dim(3);
q.Dim(4); q.Dim(4);
qdot.Dim(4); qdot.Dim(4);
PKCN.Identity(); PKCN.Identity();
PKCK.Identity(); PKCK.Identity();
w.Dim(3); w.Dim(3);
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
// Setting up Inertial Frame, gravity and Origin // Setting up Inertial Frame, gravity and Origin
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
Inertial= new InertialFrame; Inertial= new InertialFrame;
AddBody(Inertial); AddBody(Inertial);
Vect3 temp1; Vect3 temp1;
temp1.Zeros(); temp1.Zeros();
((InertialFrame*) Inertial)->SetGravity(temp1); ((InertialFrame*) Inertial)->SetGravity(temp1);
origin= new FixedPoint(temp1); origin= new FixedPoint(temp1);
Inertial->AddPoint(origin); 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];
}
}
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
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 // Begin looping over each body for recursive kinematics
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
for(int i=0;i<nfree;i++){ for(int i=0;i<nfree;i++){
prev=Inertial; prev=Inertial;
point_p=origin; point_p=origin;
body = new RigidBody; body = new RigidBody;
body->mass=masstotal[mappings[i]-1]; body->mass=masstotal[mappings[i]-1];
IM(1,1)=inertia[mappings[i]-1][0]; IM(1,1)=inertia[mappings[i]-1][0];
IM(2,2)=inertia[mappings[i]-1][1]; IM(2,2)=inertia[mappings[i]-1][1];
@ -324,49 +325,49 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
IM(2,1)=IM(1,2); IM(2,1)=IM(1,2);
IM(3,1)=IM(1,3); IM(3,1)=IM(1,3);
IM(3,2)=IM(2,3); IM(3,2)=IM(2,3);
body->inertia = IM; 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];
}
//-------------------------------------------------------//
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); PKCK=T(N);
PKCN=T(N); PKCN=T(N);
q.Zeros(); q.Zeros();
EP_FromTransformation(q,N); EP_FromTransformation(q,N);
r1=PKCN*r1; r1=PKCN*r1;
r3=PKCN*r3; r3=PKCN*r3;
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
w(k)=omega[mappings[i]-1][k-1]; w(k)=omega[mappings[i]-1][k-1];
} }
Vect3 cart_r, cart_v; Vect3 cart_r, cart_v;
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
cart_r(k)=xcm[mappings[i]-1][k-1]; cart_r(k)=xcm[mappings[i]-1][k-1];
cart_v(k)=vcm[mappings[i]-1][k-1]; cart_v(k)=vcm[mappings[i]-1][k-1];
} }
w=PKCN*w; w=PKCN*w;
EP_Derivatives(q,w,qdot); EP_Derivatives(q,w,qdot);
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS // Create bodies and joints with associated properties for POEMS
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
@ -376,12 +377,12 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
body->AddPoint(point_CM); body->AddPoint(point_CM);
body->AddPoint(point_k); body->AddPoint(point_k);
body->AddPoint(point_ch); body->AddPoint(point_ch);
AddBody(body); AddBody(body);
Mat3x3 One; Mat3x3 One;
One.Identity(); One.Identity();
ColMatrix qq=Stack(q,cart_r); ColMatrix qq=Stack(q,cart_r);
ColMatrix vv=Stack(qdot,cart_v); ColMatrix vv=Stack(qdot,cart_v);
joint=new FreeBodyJoint; joint=new FreeBodyJoint;
AddJoint(joint); AddJoint(joint);
joint->SetBodies(prev,body); joint->SetBodies(prev,body);
@ -391,225 +392,225 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
joint->SetZeroOrientation(One); joint->SetZeroOrientation(One);
joint->DimQandU(7,6); joint->DimQandU(7,6);
joint->SetInitialState(qq,vv); joint->SetInitialState(qq,vv);
joint->ForwardKinematics(); joint->ForwardKinematics();
} }
for(int i = 0; i < nfree; i++) { for(int i = 0; i < nfree; i++) {
delete [] xh1[i]; delete [] xh1[i];
delete [] xh2[i]; delete [] xh2[i];
} }
delete [] xh1; delete [] xh1;
delete [] xh2; 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){ 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 // 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; Body* body = NULL;
double ** xh2; Body* prev;
Body* Inertial;
xh1 = new double*[b]; Point* origin;
xh2 = new double*[b]; Joint* joint;
Point* point_CM;
Point* point_p;
for (int i=0; i<b; i++){ Point* point_k;
xh1[i] = new double[3]; Point* point_ch = NULL;
xh2[i] = new double[3]; 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 j=0; j<3; j++){ for(int i = 0; i < b; i++){
xh1[0][j] = xcm[mapping[0]-1][j]; mappings[i] = mapping[i];
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];
}
}
//-------------------------------------------------------------------------// 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 // Begin looping over each body for recursive kinematics
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
for(int i=0;i<b;i++){ for(int i=0;i<b;i++){
if (i == 0){ if (i == 0){
prev=Inertial; prev=Inertial;
point_p=origin; point_p=origin;
} }
else{ else{
prev = body; prev = body;
point_p = point_ch; point_p = point_ch;
} }
body = new RigidBody; body = new RigidBody;
body->mass=mass[mapping[i]-1]; body->mass=mass[mapping[i]-1];
IM(1,1)=inertia[mapping[i]-1][0]; IM(1,1)=inertia[mapping[i]-1][0];
IM(2,2)=inertia[mapping[i]-1][1]; IM(2,2)=inertia[mapping[i]-1][1];
IM(3,3)=inertia[mapping[i]-1][2]; IM(3,3)=inertia[mapping[i]-1][2];
IM(1,2)=0.0; IM(1,2)=0.0;
IM(1,3)=0.0; IM(1,3)=0.0;
IM(2,3)=0.0; IM(2,3)=0.0;
IM(2,1)=IM(1,2); IM(2,1)=IM(1,2);
IM(3,1)=IM(1,3); IM(3,1)=IM(1,3);
IM(3,2)=IM(2,3); IM(3,2)=IM(2,3);
body->inertia = IM; body->inertia = IM;
//-------------------------------------------------------// //-------------------------------------------------------//
for (int k=0;k<3;k++){ for (int k=0;k<3;k++){
r1(k+1)=xh1[i][k]-xcm[mapping[i]-1][k]; r1(k+1)=xh1[i][k]-xcm[mapping[i]-1][k];
r3(k+1)=xh2[i][k]-xcm[mapping[i]-1][k]; r3(k+1)=xh2[i][k]-xcm[mapping[i]-1][k];
} }
r2.Zeros(); r2.Zeros();
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
N(k,1)=ex_space[mapping[i]-1][k-1]; N(k,1)=ex_space[mapping[i]-1][k-1];
N(k,2)=ey_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]; N(k,3)=ez_space[mapping[i]-1][k-1];
} }
if (i==0){ if (i==0){
PKCK=T(N); PKCK=T(N);
PKCN=T(N); PKCN=T(N);
q.Zeros(); q.Zeros();
EP_FromTransformation(q,N); EP_FromTransformation(q,N);
r1=PKCN*r1; r1=PKCN*r1;
r3=PKCN*r3; r3=PKCN*r3;
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
w(k)=omega[mappings[i]-1][k-1]; w(k)=omega[mappings[i]-1][k-1];
} }
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
cart_r(k)=xcm[mappings[i]-1][k-1]; cart_r(k)=xcm[mappings[i]-1][k-1];
cart_v(k)=vcm[mappings[i]-1][k-1]; cart_v(k)=vcm[mappings[i]-1][k-1];
} }
w=PKCN*w; w=PKCN*w;
EP_Derivatives(q,w,qdot); EP_Derivatives(q,w,qdot);
} }
else{ else{
PKCK=PKCN*N; PKCK=PKCN*N;
PKCN=T(N); PKCN=T(N);
q.Zeros(); q.Zeros();
EP_FromTransformation(q,PKCK); EP_FromTransformation(q,PKCK);
r1=PKCN*r1; r1=PKCN*r1;
r3=PKCN*r3; r3=PKCN*r3;
for (int k=1;k<=3;k++){ for (int k=1;k<=3;k++){
w(k)=omega[mapping[i]-1][k-1]-omega[mapping[i-1]-1][k-1]; w(k)=omega[mapping[i]-1][k-1]-omega[mapping[i-1]-1][k-1];
} }
w=PKCN*w; w=PKCN*w;
EP_Derivatives(q, w, qdot); EP_Derivatives(q, w, qdot);
} }
//-------------------------------------------------------------------------// //-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS // 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;
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;
} }

33
lib/poems/system.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: system.h * * FILE NAME: system.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -22,6 +22,7 @@
#include <iostream> #include <iostream>
#include "poemslist.h" #include "poemslist.h"
namespace POEMS {
class Body; class Body;
class Joint; class Joint;
@ -33,37 +34,37 @@
double time; double time;
List<Body> bodies; List<Body> bodies;
List<Joint> joints; List<Joint> joints;
System(); System();
~System(); ~System();
void Delete(); void Delete();
int GetNumBodies(); int GetNumBodies();
int * GetMappings(); int * GetMappings();
void AddBody(Body* body); void AddBody(Body* body);
void AddJoint(Joint* joint); void AddJoint(Joint* joint);
void SetTime(double t); void SetTime(double t);
double GetTime(); double GetTime();
void ComputeForces(); void ComputeForces();
bool ReadIn(std::istream& in); bool ReadIn(std::istream& in);
void WriteOut(std::ostream& out); void WriteOut(std::ostream& out);
void ClearBodyIDs(); void ClearBodyIDs();
void ClearJointIDs(); 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_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); 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 #endif

2
lib/poems/vect3.cpp
View File

@ -21,6 +21,8 @@
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace POEMS;
Vect3::Vect3(){ Vect3::Vect3(){
numrows = 3; numcols = 1; numrows = 3; numcols = 1;

8
lib/poems/vect3.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: vect3.h * * FILE NAME: vect3.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -23,6 +23,7 @@
#include "virtualcolmatrix.h" #include "virtualcolmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Matrix; class Matrix;
class Mat3x3; class Mat3x3;
class Mat6x6; class Mat6x6;
@ -82,7 +83,6 @@ public:
friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C); friend void FastMult(Mat3x3& A, ColMatrix& B, Vect3& C);
friend void FastAssign(ColMatrix&A, Vect3& C); friend void FastAssign(ColMatrix&A, Vect3& C);
friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C); friend void FastMult(Mat3x3& A, Vect3& B, ColMatrix& C);
}; };
}
#endif #endif

2
lib/poems/vect4.cpp
View File

@ -21,6 +21,8 @@
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace POEMS;
Vect4::Vect4(){ Vect4::Vect4(){
numrows = 4; numcols = 1; numrows = 4; numcols = 1;

9
lib/poems/vect4.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: vect4.h * * FILE NAME: vect4.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -23,6 +23,7 @@
#include "virtualcolmatrix.h" #include "virtualcolmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Matrix; class Matrix;
class Mat4x4; class Mat4x4;
@ -41,7 +42,7 @@ public:
void BasicSet_1int(int i, double value); void BasicSet_1int(int i, double value);
void BasicIncrement_1int(int i, double value); void BasicIncrement_1int(int i, double value);
void Const(double value); void Const(double value);
MatrixType GetType() const; MatrixType GetType() const;
std::ostream& WriteData(std::ostream& c) const; std::ostream& WriteData(std::ostream& c) const;
@ -69,5 +70,5 @@ public:
friend void FastAdd(Vect4& A, Vect4& B, Vect4& C); friend void FastAdd(Vect4& A, Vect4& B, Vect4& C);
friend void FastSubt(Vect4& A, Vect4& B, Vect4& C); friend void FastSubt(Vect4& A, Vect4& B, Vect4& C);
}; };
}
#endif #endif

2
lib/poems/vect6.cpp
View File

@ -21,6 +21,8 @@
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace POEMS;
Vect6::Vect6(){ Vect6::Vect6(){
numrows = 6; numcols = 1; numrows = 6; numcols = 1;

3
lib/poems/vect6.h
View File

@ -23,6 +23,7 @@
#include "virtualcolmatrix.h" #include "virtualcolmatrix.h"
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class Matrix; class Matrix;
class Mat6x6; class Mat6x6;
class ColMatrix; class ColMatrix;
@ -69,5 +70,5 @@ public:
friend void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV); friend void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV);
}; };
}
#endif #endif

2
lib/poems/virtualcolmatrix.cpp
View File

@ -20,6 +20,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
VirtualColMatrix::VirtualColMatrix(){ VirtualColMatrix::VirtualColMatrix(){
numcols = 1; numcols = 1;

38
lib/poems/virtualcolmatrix.h
View File

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

6
lib/poems/virtualmatrix.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
VirtualMatrix::VirtualMatrix(){ VirtualMatrix::VirtualMatrix(){
numrows = numcols = 0; numrows = numcols = 0;
@ -140,14 +142,14 @@ istream& VirtualMatrix::ReadData(istream& c){
// operators and functions // operators and functions
// //
ostream& operator<< (ostream& c, const VirtualMatrix& A){ //output ostream& POEMS::operator<< (ostream& c, const VirtualMatrix& A){ //output
c << A.GetType() << ' '; c << A.GetType() << ' ';
A.WriteData(c); A.WriteData(c);
c << endl; c << endl;
return c; return c;
} }
istream& operator>> (istream& c, VirtualMatrix& A){ //input istream& POEMS::operator>> (istream& c, VirtualMatrix& A){ //input
VirtualMatrix* vm; VirtualMatrix* vm;
int matrixtype; int matrixtype;
c >> matrixtype; c >> matrixtype;

3
lib/poems/virtualmatrix.h
View File

@ -20,6 +20,7 @@
#define VIRTUALMATRIX_H #define VIRTUALMATRIX_H
#include <iostream> #include <iostream>
namespace POEMS {
enum MatrixType { enum MatrixType {
MATRIX = 0, MATRIX = 0,
COLMATRIX = 1, COLMATRIX = 1,
@ -83,5 +84,5 @@ protected:
// overloaded operators // overloaded operators
std::ostream& operator<< (std::ostream& c, const VirtualMatrix& A); // output std::ostream& operator<< (std::ostream& c, const VirtualMatrix& A); // output
std::istream& operator>> (std::istream& c, VirtualMatrix& A); // input std::istream& operator>> (std::istream& c, VirtualMatrix& A); // input
}
#endif #endif

2
lib/poems/virtualrowmatrix.cpp
View File

@ -21,6 +21,8 @@
#include <cstdlib> #include <cstdlib>
using namespace std; using namespace std;
using namespace POEMS;
VirtualRowMatrix::VirtualRowMatrix(){ VirtualRowMatrix::VirtualRowMatrix(){
numrows = 1; numrows = 1;

11
lib/poems/virtualrowmatrix.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: virtualrowmatrix.h * * FILE NAME: virtualrowmatrix.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -21,8 +21,9 @@
#include "virtualmatrix.h" #include "virtualmatrix.h"
namespace POEMS {
class VirtualRowMatrix : public VirtualMatrix { class VirtualRowMatrix : public VirtualMatrix {
public: public:
VirtualRowMatrix(); VirtualRowMatrix();
~VirtualRowMatrix(); ~VirtualRowMatrix();
double& operator_2int (int i, int j); // array access double& operator_2int (int i, int j); // array access
@ -31,7 +32,7 @@ public:
double BasicGet_2int(int i, int j) const; double BasicGet_2int(int i, int j) const;
void BasicSet_2int(int i, int j, double value); void BasicSet_2int(int i, int j, double value);
void BasicIncrement_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& operator_1int (int i) = 0; // array access
virtual double Get_1int(int i) const = 0; virtual double Get_1int(int i) const = 0;
virtual void Set_1int(int i, double value) = 0; virtual void Set_1int(int i, double value) = 0;
@ -39,5 +40,5 @@ public:
virtual void BasicSet_1int(int i, double value) = 0; virtual void BasicSet_1int(int i, double value) = 0;
virtual void BasicIncrement_1int(int i, double value) = 0; virtual void BasicIncrement_1int(int i, double value) = 0;
}; };
}
#endif #endif

2
lib/poems/workspace.cpp
View File

@ -38,6 +38,8 @@
using namespace std; using namespace std;
using namespace POEMS;
void Workspace::allocateNewSystem() { void Workspace::allocateNewSystem() {
currentIndex++; currentIndex++;

67
lib/poems/workspace.h
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE * * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME * * DESCRIPTION: SEE READ-ME *
* FILE NAME: workspace.h * * FILE NAME: workspace.h *
* AUTHORS: See Author List * * AUTHORS: See Author List *
* GRANTS: See Grants List * * GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List * * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement * * LICENSE: Please see License Agreement *
@ -11,7 +11,7 @@
* ADMINISTRATOR: Prof. Kurt Anderson * * ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab * * Computational Dynamics Lab *
* Rensselaer Polytechnic Institute * * Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 * * 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu * * CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/ *_________________________________________________________________________*/
@ -19,61 +19,62 @@
#ifndef WORKSPACE_H #ifndef WORKSPACE_H
#define WORKSPACE_H #define WORKSPACE_H
namespace POEMS {
class System; class System;
class Solver; class Solver;
struct SysData{ struct SysData{
System * system; System * system;
int solver; int solver;
int integrator; int integrator;
}; };
class Workspace { class Workspace {
SysData * system; // the multibody systems data SysData * system; // the multibody systems data
int currentIndex; int currentIndex;
int maxAlloc; int maxAlloc;
public: public:
Workspace(); Workspace();
~Workspace(); ~Workspace();
double Thalf; double Thalf;
double Tfull; double Tfull;
double ConFac; double ConFac;
double KE_val; double KE_val;
int FirstTime; int FirstTime;
bool LoadFile(char* filename); bool LoadFile(char* filename);
bool SaveFile(char* filename, int index = -1); bool SaveFile(char* filename, int index = -1);
System* GetSystem(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 AddSolver(Solver* s, int index = -1);
void LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm);
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 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 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); 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(); int getNumberOfSystems();
void SetLammpsValues(double dtv, double dthalf, double tempcon); void SetLammpsValues(double dtv, double dthalf, double tempcon);
void SetKE(int temp, double SysKE); 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 RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space);
void WriteFile(char* filename); void WriteFile(char* filename);
private: private:
void allocateNewSystem(); //helper function to handle vector resizing and such for the array of system pointers void allocateNewSystem(); //helper function to handle vector resizing and such for the array of system pointers
}; };
}
#endif #endif

4
src/POEMS/fix_poems.cpp
View File

@ -20,7 +20,6 @@
#include "fix_poems.h" #include "fix_poems.h"
#include <mpi.h> #include <mpi.h>
#include <cmath> #include <cmath>
#include <cstdio>
#include <cstring> #include <cstring>
#include <cstdlib> #include <cstdlib>
#include "workspace.h" #include "workspace.h"
@ -30,7 +29,6 @@
#include "respa.h" #include "respa.h"
#include "modify.h" #include "modify.h"
#include "force.h" #include "force.h"
#include "output.h"
#include "group.h" #include "group.h"
#include "comm.h" #include "comm.h"
#include "citeme.h" #include "citeme.h"
@ -265,7 +263,7 @@ FixPOEMS::FixPOEMS(LAMMPS *lmp, int narg, char **arg) :
// create POEMS instance // create POEMS instance
poems = new Workspace; poems = new POEMS::Workspace;
// compute per body forces and torques inside final_integrate() by default // compute per body forces and torques inside final_integrate() by default

3
src/POEMS/fix_poems.h
View File

@ -22,6 +22,7 @@ FixStyle(poems,FixPOEMS)
#include "fix.h" #include "fix.h"
namespace POEMS { class Workspace; }
namespace LAMMPS_NS { namespace LAMMPS_NS {
class FixPOEMS : public Fix { class FixPOEMS : public Fix {
@ -94,7 +95,7 @@ class FixPOEMS : public Fix {
// POEMS object // POEMS object
class Workspace *poems; POEMS::Workspace *poems;
// internal class functions // internal class functions