ICODE-ADC/lammps
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

replace tabs and remove trailing whitespace in lib folder with updated script

Browse Source
This commit is contained in:
Axel Kohlmeyer
2021-08-22 20:45:24 -04:00
parent 30821b37e5
commit 92b5b159e5
311 changed files with 9176 additions and 9176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
lib/poems/sphericaljoint.cpp
View File

@ -3,7 +3,7 @@
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: sphericaljoint.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,10 +11,10 @@
* 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 *
*_________________________________________________________________________*/
#include "sphericaljoint.h"
#include "point.h"
@ -44,24 +44,24 @@ bool SphericalJoint::ReadInJointData(std::istream& in){
void SphericalJoint::WriteOutJointData(std::ostream& out){
}
Matrix SphericalJoint::GetForward_sP(){
Matrix SphericalJoint::GetForward_sP(){
Mat3x3 sPa,sPl;
Matrix sP(6,3);
sPa.Identity();
sPa.Identity();
sPl.Zeros();
Vect3 temp = -(point2->position);
Vect3 temp = -(point2->position);
sPl(1,2) = temp(3);
sPl(1,3) = -temp(2);
sPl(2,1) = -temp(3);
sPl(2,3) = temp(1);
sPl(3,1) = temp(2);
sPl(3,1) = temp(2);
sPl(3,2) = -temp(1);
sP=Stack(sPa,sPl);
return sP;
sP=Stack(sPa,sPl);
return sP;
}
void SphericalJoint::UpdateForward_sP( Matrix& sP){
@ -70,128 +70,128 @@ void SphericalJoint::UpdateForward_sP( Matrix& sP){
}
Matrix SphericalJoint::GetBackward_sP(){
cout<<" -----------"<<endl;
cout<<"Am I coming here "<<endl;
cout<<" -----------"<<endl;
cout<<" -----------"<<endl;
cout<<"Am I coming here "<<endl;
cout<<" -----------"<<endl;
Mat3x3 sPa,sPl;
Matrix sP;
sPa.Identity();
sPl.Zeros();
sPl(3,2)=(point2->position(1));
sPl(2,3)=-(point2->position(1));
sPa.Identity();
sPl.Zeros();
sPl(3,2)=(point2->position(1));
sPl(2,3)=-(point2->position(1));
sP=Stack(sPa,sPl);
return sP;
return sP;
}
void SphericalJoint::UpdateBackward_sP( Matrix& sP){
void SphericalJoint::UpdateBackward_sP( Matrix& sP){
// sP is constant, do nothing.
}
void SphericalJoint::ComputeLocalTransform(){
Mat3x3 ko_C_k;
// Obtain the transformation matrix from euler parameters
EP_Transformation(q, ko_C_k);
FastMult(pk_C_ko,ko_C_k,pk_C_k);
}
Mat3x3 ko_C_k;
// Obtain the transformation matrix from euler parameters
EP_Transformation(q, ko_C_k);
FastMult(pk_C_ko,ko_C_k,pk_C_k);
}
void SphericalJoint::ForwardKinematics(){
Vect3 result1,result2,result3,result4,result5;
Vect3 pk_w_k;
//cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
EP_Normalize(q);
// orientations
ComputeForwardTransforms();
ComputeForwardTransforms();
//----------------------------------//
// COMPUTE POSITION VECTOR R12 aka GAMMA
FastNegMult(pk_C_k,point2->position,result1); // parents basis
FastAdd(result1,point1->position,r12);
// compute position vector r21
FastNegMult(k_C_pk,r12,r21);
// COMPUTE POSITION VECTOR R12 aka GAMMA
FastNegMult(pk_C_k,point2->position,result1); // parents basis
FastAdd(result1,point1->position,r12);
// compute position vector r21
FastNegMult(k_C_pk,r12,r21);
//----------------------------------//
// COMPUTE GLOBAL LOCATION
FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);
FastAdd(result1,body1->r,result1);
FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);
FastAdd(result1,result2,body2->r);
qdot_to_u(q, u, qdot);
// COMPUTE GLOBAL LOCATION
FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);
FastAdd(result1,body1->r,result1);
FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);
FastAdd(result1,result2,body2->r);
qdot_to_u(q, u, qdot);
//-----------------------------------
// angular velocities
//-----------------------------------
// angular velocities
FastAssign(u,pk_w_k);
FastTMult(pk_C_k,body1->omega_k,result1);
FastTMult(pk_C_k,body1->omega_k,result1);
FastAdd(result1,pk_w_k,body2->omega_k);
FastMult(body2->n_C_k,body2->omega_k,body2->omega);
FastMult(body2->n_C_k,body2->omega_k,body2->omega);
//-----------------------------------
// compute velocities
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
//-----------------------------------
// compute velocities
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
FastAdd(body1->v_k,result1,result2);
FastTMult(pk_C_k,result2,result1); // In body basis
FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);
FastAdd(result1,result2,body2->v_k); // In body basis
FastMult(body2->n_C_k,body2->v_k,body2->v);
FastTMult(pk_C_k,result2,result1); // In body basis
FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);
FastAdd(result1,result2,body2->v_k); // In body basis
FastMult(body2->n_C_k,body2->v_k,body2->v);
//------------------------------------------
//Compute the KE
//Compute the KE
Matrix tempke;
tempke = T(body2->v)*(body2->v);
tempke = T(body2->v)*(body2->v);
double ke = 0.0;
ke = body2->mass*tempke(1,1);
ke = body2->mass*tempke(1,1);
FastMult(body2->inertia,body2->omega_k,result1);
tempke= T(body2->omega_k)*result1;
tempke= T(body2->omega_k)*result1;
ke = 0.5*ke + 0.5*tempke(1,1);
body2->KE=ke;
//-----------------------------------
// compute state explicit angular acceleration // Has to be in body basis
FastTMult(pk_C_k,body1->alpha_t,result2);
//-----------------------------------
// compute state explicit angular acceleration // Has to be in body basis
FastTMult(pk_C_k,body1->alpha_t,result2);
FastCross(body2->omega_k,pk_w_k,result1);
FastAdd(result1,result2,body2->alpha_t);
//-----------------------------------
// compute state explicit acceleration
// NEED TO DO THIS COMPLETELY IN BODY BASIS
FastAdd(result1,result2,body2->alpha_t);
//-----------------------------------
// compute state explicit acceleration
// NEED TO DO THIS COMPLETELY IN BODY BASIS
FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
FastCross(body1->omega_k,result1,result2);
FastTMult(pk_C_k,result2,result1);
//FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);
FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);
FastCross(body2->omega_k,result3,result2);
FastAdd(result1,result2,result3); //wxwxr in body basis
FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4);
FastTMult(pk_C_k,result4,result5);
FastCross(body1->omega_k,result1,result2);
FastTMult(pk_C_k,result2,result1);
//FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);
FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);
FastCross(body2->omega_k,result3,result2);
FastAdd(result1,result2,result3); //wxwxr in body basis
FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4);
FastTMult(pk_C_k,result4,result5);
FastAssign(result5,result4);
FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);
FastAdd(result2,result4,result1); //alphaxr in body basis
FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);
FastAdd(result2,result4,result1); //alphaxr in body basis
FastTMult(pk_C_k,body1->a_t,result2);
FastTripleSum(result3,result1,result2,body2->a_t); // in body basis
//-----------------------------------
FastTripleSum(result3,result1,result2,body2->a_t); // in body basis
//-----------------------------------
}
// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
@ -202,13 +202,13 @@ void SphericalJoint::BackwardKinematics(){
// orientations
ComputeBackwardTransforms();
// compute position vector r21
//r21 = point2->position - k_C_pk * point1->position;
FastMult(k_C_pk,point1->position,result1);
FastSubt(point2->position,result1,r21);
// compute position vector r21
FastNegMult(pk_C_k,r21,r12);
@ -227,7 +227,7 @@ void SphericalJoint::BackwardKinematics(){
EP_Derivatives(q,u,qdot);
// angular velocities
FastMult(body2->n_C_k,u,result2);
FastAdd(body2->omega,result2,body1->omega);
FastAssign(u,k_w_pk);
@ -247,7 +247,7 @@ void SphericalJoint::BackwardKinematics(){
FastCross(body1->omega_k,k_w_pk,result1);
FastMult(pk_C_k,body2->alpha_t,result2);
FastAdd(result1,result2,body1->alpha_t);
// compute state explicit acceleration
FastCross(body2->alpha_t,point2->position,result1);
FastCross(body2->omega_k,point2->position,result2);
@ -260,5 +260,5 @@ void SphericalJoint::BackwardKinematics(){
FastCross(body1->omega_k,result2,result3);
FastTripleSum(result5,result1,result3,body1->a_t);
}