replace tabs and remove trailing whitespace in lib folder with updated script
This commit is contained in:
Axel Kohlmeyer
@ -3,7 +3,7 @@
|
||||
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
|
||||
* DESCRIPTION: SEE READ-ME *
|
||||
* FILE NAME: system.cpp *
|
||||
* AUTHORS: See Author List *
|
||||
* AUTHORS: See Author List *
|
||||
* GRANTS: See Grants List *
|
||||
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
|
||||
* LICENSE: Please see License Agreement *
|
||||
@ -11,7 +11,7 @@
|
||||
* ADMINISTRATOR: Prof. Kurt Anderson *
|
||||
* Computational Dynamics Lab *
|
||||
* Rensselaer Polytechnic Institute *
|
||||
* 110 8th St. Troy NY 12180 *
|
||||
* 110 8th St. Troy NY 12180 *
|
||||
* CONTACT: anderk5@rpi.edu *
|
||||
*_________________________________________________________________________*/
|
||||
|
||||
@ -23,7 +23,7 @@
|
||||
|
||||
|
||||
System::System(){
|
||||
mappings = nullptr;
|
||||
mappings = nullptr;
|
||||
}
|
||||
|
||||
System::~System(){
|
||||
@ -42,7 +42,7 @@ int System::GetNumBodies(){
|
||||
|
||||
int * System::GetMappings()
|
||||
{
|
||||
return mappings;
|
||||
return mappings;
|
||||
}
|
||||
|
||||
void System::AddBody(Body* body){
|
||||
@ -61,16 +61,16 @@ double System::GetTime(){
|
||||
return time;
|
||||
}
|
||||
|
||||
void System::ComputeForces(){
|
||||
// NOT DOING ANYTHING AT THIS 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;
|
||||
int index;
|
||||
|
||||
// get number of bodies
|
||||
in >> numbodies;
|
||||
@ -148,12 +148,12 @@ bool System::ReadIn(istream& in){
|
||||
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));
|
||||
@ -186,7 +186,7 @@ void System::WriteOut(ostream& out){
|
||||
|
||||
// set the body ID for later identification
|
||||
body->SetID(i);
|
||||
|
||||
|
||||
// write out the data
|
||||
body->WriteOut(out);
|
||||
|
||||
@ -194,7 +194,7 @@ void System::WriteOut(ostream& out){
|
||||
}
|
||||
|
||||
// number of joints
|
||||
out << joints.GetNumElements() << endl;
|
||||
out << joints.GetNumElements() << endl;
|
||||
|
||||
// joints loop
|
||||
i = 0;
|
||||
@ -209,7 +209,7 @@ void System::WriteOut(ostream& out){
|
||||
|
||||
// write out the data
|
||||
joint->WriteOut(out);
|
||||
|
||||
|
||||
i++; j_ele = j_ele->next;
|
||||
}
|
||||
}
|
||||
@ -235,68 +235,68 @@ void System::ClearJointIDs(){
|
||||
|
||||
void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstotal, double **&inertia, double **&xcm, double **&vcm, double **&omega, double **&ex_space, double **&ey_space, double **&ez_space){
|
||||
|
||||
//-------------------------------------------------------------------------//
|
||||
// Declaring Temporary Entities
|
||||
//-------------------------------------------------------------------------//
|
||||
//-------------------------------------------------------------------------//
|
||||
// Declaring Temporary Entities
|
||||
//-------------------------------------------------------------------------//
|
||||
Body* body = nullptr;
|
||||
Body* prev;
|
||||
Body* Inertial;
|
||||
Point* origin;
|
||||
Joint* joint;
|
||||
Point* point_CM;
|
||||
Point* point_p;
|
||||
Point* point_k;
|
||||
Point* point_CM;
|
||||
Point* point_p;
|
||||
Point* point_k;
|
||||
Point* point_ch = nullptr;
|
||||
Vect3 r1,r2,r3,v1,v2,v3;
|
||||
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];
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
//-------------------------------------------------------------------------//
|
||||
// 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];
|
||||
}
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------//
|
||||
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;
|
||||
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];
|
||||
@ -307,49 +307,49 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
||||
IM(2,1)=IM(1,2);
|
||||
IM(3,1)=IM(1,3);
|
||||
IM(3,2)=IM(2,3);
|
||||
body->inertia = IM;
|
||||
body->inertia = IM;
|
||||
|
||||
//-------------------------------------------------------//
|
||||
|
||||
|
||||
for (int k=0;k<3;k++){
|
||||
r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];
|
||||
r3(k+1) = xcm[mappings[i]-1][k];
|
||||
r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k];
|
||||
}
|
||||
|
||||
r2.Zeros();
|
||||
|
||||
for (int k=1;k<=3;k++){
|
||||
N(k,1)=ex_space[mappings[i]-1][k-1];
|
||||
N(k,2)=ey_space[mappings[i]-1][k-1];
|
||||
N(k,3)=ez_space[mappings[i]-1][k-1];
|
||||
}
|
||||
|
||||
//-------------------------------------------------------//
|
||||
|
||||
|
||||
for (int k=0;k<3;k++){
|
||||
r1(k+1)=xh1[i][k]-xcm[mappings[i]-1][k];
|
||||
r3(k+1) = xcm[mappings[i]-1][k];
|
||||
r3(k+1)=xh2[i][k]-xcm[mappings[i]-1][k];
|
||||
}
|
||||
|
||||
r2.Zeros();
|
||||
|
||||
for (int k=1;k<=3;k++){
|
||||
N(k,1)=ex_space[mappings[i]-1][k-1];
|
||||
N(k,2)=ey_space[mappings[i]-1][k-1];
|
||||
N(k,3)=ez_space[mappings[i]-1][k-1];
|
||||
}
|
||||
|
||||
PKCK=T(N);
|
||||
PKCN=T(N);
|
||||
|
||||
q.Zeros();
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
|
||||
//-------------------------------------------------------------------------//
|
||||
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
|
||||
//-------------------------------------------------------------------------//
|
||||
|
||||
@ -359,12 +359,12 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
||||
body->AddPoint(point_CM);
|
||||
body->AddPoint(point_k);
|
||||
body->AddPoint(point_ch);
|
||||
AddBody(body);
|
||||
|
||||
AddBody(body);
|
||||
|
||||
Mat3x3 One;
|
||||
One.Identity();
|
||||
ColMatrix qq=Stack(q,cart_r);
|
||||
ColMatrix vv=Stack(qdot,cart_v);
|
||||
One.Identity();
|
||||
ColMatrix qq=Stack(q,cart_r);
|
||||
ColMatrix vv=Stack(qdot,cart_v);
|
||||
joint=new FreeBodyJoint;
|
||||
AddJoint(joint);
|
||||
joint->SetBodies(prev,body);
|
||||
@ -374,225 +374,225 @@ void System::Create_DegenerateSystem(int& nfree, int*freelist, double *&masstota
|
||||
joint->SetZeroOrientation(One);
|
||||
joint->DimQandU(7,6);
|
||||
joint->SetInitialState(qq,vv);
|
||||
joint->ForwardKinematics();
|
||||
joint->ForwardKinematics();
|
||||
}
|
||||
for(int i = 0; i < nfree; i++) {
|
||||
delete [] xh1[i];
|
||||
delete [] xh2[i];
|
||||
delete [] xh1[i];
|
||||
delete [] xh2[i];
|
||||
}
|
||||
delete [] xh1;
|
||||
delete [] xh2;
|
||||
delete [] xh2;
|
||||
}
|
||||
|
||||
|
||||
void System::Create_System_LAMMPS(int numbodies, double *mass,double **inertia, double ** xcm, double ** xjoint,double **vcm,double **omega,double **ex_space, double **ey_space, double **ez_space, int b, int * mapping, int count){
|
||||
|
||||
//-------------------------------------------------------------------------//
|
||||
// Declaring Temporary Entities
|
||||
//-------------------------------------------------------------------------//
|
||||
|
||||
Body* body = nullptr;
|
||||
Body* prev;
|
||||
Body* Inertial;
|
||||
Point* origin;
|
||||
Joint* joint;
|
||||
Point* point_CM;
|
||||
Point* point_p;
|
||||
Point* point_k;
|
||||
Point* point_ch = nullptr;
|
||||
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);
|
||||
//-------------------------------------------------------------------------//
|
||||
//-------------------------------------------------------------------------//
|
||||
// Declaring Temporary Entities
|
||||
//-------------------------------------------------------------------------//
|
||||
|
||||
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];
|
||||
}
|
||||
}
|
||||
Body* body = nullptr;
|
||||
Body* prev;
|
||||
Body* Inertial;
|
||||
Point* origin;
|
||||
Joint* joint;
|
||||
Point* point_CM;
|
||||
Point* point_p;
|
||||
Point* point_k;
|
||||
Point* point_ch = nullptr;
|
||||
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
|
||||
//-------------------------------------------------------------------------//
|
||||
//-------------------------------------------------------------------------//
|
||||
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;
|
||||
|
||||
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