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