jianmu
/
lammps
forked from lijiext/lammps
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
lammps/lib/poems/system.cpp

597 lines
14 KiB
C++
Raw Blame History

/*
*_________________________________________________________________________*
* 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 View Git Blame Copy Permalink