please POEMS library in namespace POEMS and remove "using namespace" from headers
This commit is contained in:
@ -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
|
||||||
|
|||||||
@ -17,53 +17,55 @@
|
|||||||
|
|
||||||
#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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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){
|
||||||
@ -40,7 +42,7 @@ void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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];
|
||||||
@ -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,7 +119,7 @@ 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];
|
||||||
@ -128,7 +130,7 @@ 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++){
|
||||||
@ -150,7 +152,7 @@ void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq, ColMatrix&
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
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){
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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];
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -24,6 +24,8 @@
|
|||||||
#include "virtualmatrix.h"
|
#include "virtualmatrix.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
|
|
||||||
FixedPoint::FixedPoint(){
|
FixedPoint::FixedPoint(){
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -29,6 +29,8 @@
|
|||||||
#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);
|
||||||
@ -73,7 +75,7 @@ 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -87,7 +89,7 @@ void FreeBodyJoint::UpdateBackward_sP( Matrix& sP){
|
|||||||
}
|
}
|
||||||
|
|
||||||
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
|
||||||
@ -155,6 +157,6 @@ void FreeBodyJoint::ForwardKinematics(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
void FreeBodyJoint::BackwardKinematics(){
|
void FreeBodyJoint::BackwardKinematics(){
|
||||||
cout<<"Did I come here "<<endl;
|
std::cout<<"Did I come here "<<std::endl;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -23,7 +23,7 @@
|
|||||||
#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();
|
||||||
@ -40,5 +40,5 @@ public:
|
|||||||
void ForwardKinematics();
|
void ForwardKinematics();
|
||||||
void BackwardKinematics();
|
void BackwardKinematics();
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
@ -76,7 +77,6 @@ public:
|
|||||||
|
|
||||||
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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -21,6 +21,7 @@
|
|||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include "virtualmatrix.h"
|
#include "virtualmatrix.h"
|
||||||
|
|
||||||
|
namespace POEMS {
|
||||||
class Vect4;
|
class Vect4;
|
||||||
class Matrix;
|
class Matrix;
|
||||||
|
|
||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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];
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -29,6 +29,8 @@
|
|||||||
#include "vect3.h"
|
#include "vect3.h"
|
||||||
#include "virtualmatrix.h"
|
#include "virtualmatrix.h"
|
||||||
|
|
||||||
|
using namespace POEMS;
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
|
||||||
MixedJoint::MixedJoint(){
|
MixedJoint::MixedJoint(){
|
||||||
|
|||||||
@ -24,7 +24,7 @@
|
|||||||
#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;
|
||||||
@ -46,5 +46,5 @@ public:
|
|||||||
void ForwardKinematics();
|
void ForwardKinematics();
|
||||||
void BackwardKinematics();
|
void BackwardKinematics();
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -18,6 +18,7 @@
|
|||||||
#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
|
||||||
|
|||||||
@ -30,6 +30,8 @@
|
|||||||
#include "colmatrix.h"
|
#include "colmatrix.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
OnBody::OnBody(){
|
OnBody::OnBody(){
|
||||||
|
|||||||
@ -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 sI; // spatial inertias
|
||||||
Mat6x6 sIhat; // recursive spatial inertias
|
Mat6x6 sIhat; // recursive spatial inertias
|
||||||
Mat6x6 sSC; // spatial shift
|
Mat6x6 sSC; // spatial shift
|
||||||
Mat6x6 sT; // spatial triangularization
|
Mat6x6 sT; // spatial triangularization
|
||||||
|
|
||||||
Vect6 sF; // spatial forces
|
Vect6 sF; // spatial forces
|
||||||
Vect6 sFhat; // recursive spatial forces
|
Vect6 sFhat; // recursive spatial forces
|
||||||
Vect6 sAhat; // recursive spatial acceleration
|
Vect6 sAhat; // recursive spatial acceleration
|
||||||
|
|
||||||
Matrix sP; // spatial partial velocities
|
Matrix sP; // spatial partial velocities
|
||||||
Matrix sM; // triangularized mass matrix diagonal elements
|
Matrix sM; // triangularized mass matrix diagonal elements
|
||||||
Matrix sMinv; // inverse of sM
|
Matrix sMinv; // inverse of sM
|
||||||
Matrix sPsMinv;
|
Matrix sPsMinv;
|
||||||
Matrix sIhatsP;
|
Matrix sIhatsP;
|
||||||
|
|
||||||
// states and state derivatives
|
// states and state derivatives
|
||||||
ColMatrix* q;
|
ColMatrix* q;
|
||||||
ColMatrix* u;
|
ColMatrix* u;
|
||||||
ColMatrix* qdot;
|
ColMatrix* qdot;
|
||||||
ColMatrix* udot;
|
ColMatrix* udot;
|
||||||
ColMatrix* qdotdot;
|
ColMatrix* qdotdot;
|
||||||
|
|
||||||
ColMatrix* r;
|
ColMatrix* r;
|
||||||
ColMatrix* acc;
|
ColMatrix* acc;
|
||||||
ColMatrix* ang;
|
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
|
||||||
|
|||||||
@ -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,7 +39,7 @@ 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
|
||||||
@ -65,10 +67,10 @@ void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC){
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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();
|
void DeleteModel();
|
||||||
int CreateTopologyArray(int i, OnBody* body);
|
int CreateTopologyArray(int i, OnBody* body);
|
||||||
void CreateStateMatrixMaps();
|
void CreateStateMatrixMaps();
|
||||||
void GetType();
|
void GetType();
|
||||||
public:
|
public:
|
||||||
OnSolver();
|
OnSolver();
|
||||||
~OnSolver();
|
~OnSolver();
|
||||||
void CreateModel();
|
void CreateModel();
|
||||||
void Solve(double time, Matrix& FF);
|
void Solve(double time, Matrix& FF);
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -18,6 +18,8 @@
|
|||||||
|
|
||||||
#include "particle.h"
|
#include "particle.h"
|
||||||
|
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
Particle::Particle(){
|
Particle::Particle(){
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -23,7 +23,7 @@
|
|||||||
|
|
||||||
#include "body.h"
|
#include "body.h"
|
||||||
|
|
||||||
|
namespace POEMS {
|
||||||
class Particle : public Body {
|
class Particle : public Body {
|
||||||
public:
|
public:
|
||||||
Particle();
|
Particle();
|
||||||
@ -32,5 +32,5 @@ public:
|
|||||||
bool ReadInBodyData(std::istream& in);
|
bool ReadInBodyData(std::istream& in);
|
||||||
void WriteOutBodyData(std::ostream& out);
|
void WriteOutBodyData(std::ostream& out);
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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;
|
||||||
@ -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,14 +144,14 @@ 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;
|
||||||
@ -160,7 +159,7 @@ template<class S> ListElement<S>* List<S>::Append(S* v){
|
|||||||
|
|
||||||
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,7 +189,7 @@ 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);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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");
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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,
|
||||||
|
|||||||
@ -18,29 +18,30 @@
|
|||||||
#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
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -22,7 +22,7 @@
|
|||||||
#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
|
||||||
@ -45,5 +45,5 @@ public:
|
|||||||
|
|
||||||
// global point functions
|
// global point functions
|
||||||
Point* NewPoint(int type);
|
Point* NewPoint(int type);
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
#include "virtualmatrix.h"
|
#include "virtualmatrix.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
|
|
||||||
RigidBody::RigidBody(){
|
RigidBody::RigidBody(){
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -20,6 +20,8 @@
|
|||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "onsolver.h"
|
#include "onsolver.h"
|
||||||
|
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
Solver::Solver(){
|
Solver::Solver(){
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
@ -55,5 +56,5 @@ public:
|
|||||||
ColMatMap* GetStateDerivative();
|
ColMatMap* GetStateDerivative();
|
||||||
ColMatMap* GetStateDerivativeDerivative();
|
ColMatMap* GetStateDerivativeDerivative();
|
||||||
};
|
};
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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){
|
||||||
@ -79,7 +80,7 @@ double System::GetTime(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
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){
|
||||||
@ -268,11 +269,11 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
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);
|
||||||
@ -293,25 +294,25 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
origin= new FixedPoint(temp1);
|
origin= new FixedPoint(temp1);
|
||||||
Inertial->AddPoint(origin);
|
Inertial->AddPoint(origin);
|
||||||
//-------------------------------------------------------------------------//
|
//-------------------------------------------------------------------------//
|
||||||
double ** xh1 = new double*[nfree];
|
double ** xh1 = new double*[nfree];
|
||||||
double ** xh2 = new double*[nfree];
|
double ** xh2 = new double*[nfree];
|
||||||
|
|
||||||
for (int i=0; i<nfree; i++){
|
for (int i=0; i<nfree; i++){
|
||||||
xh1[i] = new double[3];
|
xh1[i] = new double[3];
|
||||||
xh2[i] = new double[3];
|
xh2[i] = new double[3];
|
||||||
}
|
}
|
||||||
for (int i=0; i<nfree; i++){
|
for (int i=0; i<nfree; i++){
|
||||||
for (int j=0; j<3; j++){
|
for (int j=0; j<3; j++){
|
||||||
xh1[i][j] = xcm[mappings[i]-1][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];
|
||||||
@ -329,11 +330,11 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
//-------------------------------------------------------//
|
//-------------------------------------------------------//
|
||||||
|
|
||||||
|
|
||||||
for (int k=0;k<3;k++){
|
for (int k=0;k<3;k++){
|
||||||
r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];
|
r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];
|
||||||
r3(k+1) = xcm[mappings[i]-1][k];
|
r3(k+1) = xcm[mappings[i]-1][k];
|
||||||
r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k];
|
r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k];
|
||||||
}
|
}
|
||||||
|
|
||||||
r2.Zeros();
|
r2.Zeros();
|
||||||
|
|
||||||
@ -358,12 +359,12 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
|
|
||||||
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);
|
||||||
|
|
||||||
|
|
||||||
//-------------------------------------------------------------------------//
|
//-------------------------------------------------------------------------//
|
||||||
@ -380,7 +381,7 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
|
|
||||||
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);
|
||||||
@ -394,8 +395,8 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
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;
|
||||||
@ -404,212 +405,212 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
|||||||
|
|
||||||
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* 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;
|
||||||
Vect3 cart_r, cart_v;
|
Vect3 cart_r, cart_v;
|
||||||
mappings = new int[b];
|
mappings = new int[b];
|
||||||
for(int i = 0; i < b; i++){
|
for(int i = 0; i < b; i++){
|
||||||
mappings[i] = mapping[i];
|
mappings[i] = mapping[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;
|
double ** xh1;
|
||||||
double ** xh2;
|
double ** xh2;
|
||||||
|
|
||||||
xh1 = new double*[b];
|
xh1 = new double*[b];
|
||||||
xh2 = new double*[b];
|
xh2 = new double*[b];
|
||||||
|
|
||||||
|
|
||||||
for (int i=0; i<b; i++){
|
for (int i=0; i<b; i++){
|
||||||
xh1[i] = new double[3];
|
xh1[i] = new double[3];
|
||||||
xh2[i] = new double[3];
|
xh2[i] = new double[3];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
for (int j=0; j<3; j++){
|
for (int j=0; j<3; j++){
|
||||||
xh1[0][j] = xcm[mapping[0]-1][j];
|
xh1[0][j] = xcm[mapping[0]-1][j];
|
||||||
xh2[b-1][j] = xcm[mapping[b-1]-1][j];
|
xh2[b-1][j] = xcm[mapping[b-1]-1][j];
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i=0; i<b-1; i++){
|
for (int i=0; i<b-1; i++){
|
||||||
for (int j=0; j<3; j++){
|
for (int j=0; j<3; j++){
|
||||||
xh1[i+1][j] = xjoint[mapping[i]-count-1][j];
|
xh1[i+1][j] = xjoint[mapping[i]-count-1][j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i=0; i<b-1; i++){
|
for (int i=0; i<b-1; i++){
|
||||||
for (int j=0; j<3; j++){
|
for (int j=0; j<3; j++){
|
||||||
xh2[i][j] = xjoint[mapping[i]-count-1][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_CM = new FixedPoint(r2);
|
||||||
point_k = new FixedPoint(r1);
|
point_k = new FixedPoint(r1);
|
||||||
point_ch = new FixedPoint(r3);
|
point_ch = new FixedPoint(r3);
|
||||||
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();
|
||||||
if (i==0){
|
if (i==0){
|
||||||
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);
|
||||||
body->AddJoint(joint);
|
body->AddJoint(joint);
|
||||||
prev->AddJoint(joint);
|
prev->AddJoint(joint);
|
||||||
joint->SetPoints(point_p,point_k);
|
joint->SetPoints(point_p,point_k);
|
||||||
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();
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
joint= new SphericalJoint;
|
joint= new SphericalJoint;
|
||||||
AddJoint(joint);
|
AddJoint(joint);
|
||||||
joint->SetBodies(prev,body);
|
joint->SetBodies(prev,body);
|
||||||
body->AddJoint(joint);
|
body->AddJoint(joint);
|
||||||
prev->AddJoint(joint);
|
prev->AddJoint(joint);
|
||||||
joint->SetPoints(point_p,point_k);
|
joint->SetPoints(point_p,point_k);
|
||||||
joint->SetZeroOrientation(One);
|
joint->SetZeroOrientation(One);
|
||||||
joint->DimQandU(4,3);
|
joint->DimQandU(4,3);
|
||||||
joint->SetInitialState(q,qdot);
|
joint->SetInitialState(q,qdot);
|
||||||
joint->ForwardKinematics();
|
joint->ForwardKinematics();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for(int i = 0; i < b; i++)
|
for(int i = 0; i < b; i++)
|
||||||
{
|
{
|
||||||
delete [] xh1[i];
|
delete [] xh1[i];
|
||||||
delete [] xh2[i];
|
delete [] xh2[i];
|
||||||
}
|
}
|
||||||
delete [] xh1;
|
delete [] xh1;
|
||||||
delete [] xh2;
|
delete [] xh2;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -22,6 +22,7 @@
|
|||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include "poemslist.h"
|
#include "poemslist.h"
|
||||||
|
|
||||||
|
namespace POEMS {
|
||||||
class Body;
|
class Body;
|
||||||
class Joint;
|
class Joint;
|
||||||
|
|
||||||
@ -65,5 +66,5 @@
|
|||||||
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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -21,6 +21,7 @@
|
|||||||
|
|
||||||
#include "virtualmatrix.h"
|
#include "virtualmatrix.h"
|
||||||
|
|
||||||
|
namespace POEMS {
|
||||||
class VirtualRowMatrix : public VirtualMatrix {
|
class VirtualRowMatrix : public VirtualMatrix {
|
||||||
public:
|
public:
|
||||||
VirtualRowMatrix();
|
VirtualRowMatrix();
|
||||||
@ -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
|
||||||
|
|||||||
@ -38,6 +38,8 @@
|
|||||||
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
using namespace POEMS;
|
||||||
|
|
||||||
|
|
||||||
void Workspace::allocateNewSystem() {
|
void Workspace::allocateNewSystem() {
|
||||||
currentIndex++;
|
currentIndex++;
|
||||||
|
|||||||
@ -19,19 +19,20 @@
|
|||||||
#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();
|
||||||
@ -41,7 +42,7 @@ public:
|
|||||||
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);
|
||||||
|
|
||||||
@ -60,20 +61,20 @@ public:
|
|||||||
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
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user