forked from lijiext/lammps
599 lines
14 KiB
C++
599 lines
14 KiB
C++
/*
|
|
*_________________________________________________________________________*
|
|
* 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;
|
|
delete [] bodyarray;
|
|
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)){
|
|
delete [] bodyarray;
|
|
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;
|
|
|
|
}
|