Skip to content

Commit

Permalink
Added option to moving frame load to specify the angular velocity in …
Browse files Browse the repository at this point in the history 
…the moving frame instead of the fixed frame.
  • Loading branch information
@SteveMaas1978
SteveMaas1978 committed Sep 10, 2024
1 parent dfda148 commit a7cb50b
Show file tree
Hide file tree
Showing 4 changed files with 160 additions and 85 deletions.
105 changes: 71 additions & 34 deletions FEBioFluid/FEFluidMovingFrameLoad.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,10 @@ BEGIN_FECORE_CLASS(FEFluidMovingFrameLoad, FEBodyForce)
ADD_PARAMETER(m_at.x, "ax");
ADD_PARAMETER(m_at.y, "ay");
ADD_PARAMETER(m_at.z, "az");
ADD_PARAMETER(m_wt.x, "wx");
ADD_PARAMETER(m_wt.y, "wy");
ADD_PARAMETER(m_wt.z, "wz");
ADD_PARAMETER(m_wi.x, "wx");
ADD_PARAMETER(m_wi.y, "wy");
ADD_PARAMETER(m_wi.z, "wz");
ADD_PARAMETER(m_frame, "frame")->setEnums("fixed\0moving\0");

ADD_PARAMETER(m_wt, "wt")->setLongName("Angular velocity in fixed frame")->setUnits(UNIT_ANGULAR_VELOCITY)->SetFlags(FE_PARAM_HIDDEN);
ADD_PARAMETER(m_Wt, "Wt")->setLongName("Angular velocity in moving frame")->setUnits(UNIT_ANGULAR_VELOCITY)->SetFlags(FE_PARAM_HIDDEN);
Expand All @@ -43,13 +44,16 @@ END_FECORE_CLASS();

FEFluidMovingFrameLoad::FEFluidMovingFrameLoad(FEModel* fem) : FEBodyForce(fem)
{
m_frame = 0; // assume angular velocity is input in fixed frame
}

void FEFluidMovingFrameLoad::Activate()
{
m_wp = m_wt;
m_ap = m_at;
m_wp = m_wt;
m_Wp = m_Wt;
m_alp = m_alt;
m_Alp = m_Alt;
m_qp = m_qt;
FEBodyForce::Activate();
}
Expand All @@ -60,29 +64,68 @@ void FEFluidMovingFrameLoad::PrepStep()
double dt = tp.timeIncrement;
double alpha = tp.alpha;

// evaluate quantities at current time (t) and intermediate time
if (m_frame == 0)
{
m_wt = m_wi; // input is ang vel in fixed frame

// current quantities
m_alp = m_alt;
m_alt = (m_wt - m_wp) / dt;
quatd wt(m_wt.x, m_wt.y, m_wt.z, 0.0);
m_qt = m_qp + (wt * m_qp) * (0.5 * dt); m_qt.MakeUnit();

// intermediate quantities
m_al = m_alt * alpha + m_alp * (1.0 - alpha);
m_w = m_wt * alpha + m_wp * (1.0 - alpha);
quatd wa(m_w.x, m_w.y, m_w.z, 0.0);
m_q = m_qp + (wa * m_qp) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_wp = m_wt;

// quantities in body frame
quatd qti = m_qt.Inverse();
m_Wt = qti * m_wt;
m_rt = m_qt.GetRotationVector();

quatd qi = m_q.Inverse();
m_Al = qi * m_al;
m_W = qi * m_w;
}
else
{
m_Wt = m_wi; // input is ang vel in body frame

// current quantities
m_Alp = m_Alt;
m_Alt = (m_Wt - m_Wp) / dt;
quatd Wt(m_Wt.x, m_Wt.y, m_Wt.z, 0.0);
m_qt = m_qp + (m_qp * Wt) * (0.5 * dt); m_qt.MakeUnit();

// intermediate quantities
m_Al = m_Alt * alpha + m_Alp * (1.0 - alpha);
m_W = m_Wt * alpha + m_Wp * (1.0 - alpha);
quatd Wa(m_W.x, m_W.y, m_W.z, 0.0);
m_q = m_qp + (m_qp * Wa) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_Wp = m_Wt;

// quantities in fixed frame
m_wt = m_qt * m_Wt;
m_rt = m_qt.GetRotationVector();
m_alp = m_alt;
m_alt = m_qt * m_Alt;

m_al = m_q * m_Al;
m_w = m_q * m_W;
}

// linear acceleration (always assumed to be given in the fixed frame)
m_a = m_at * alpha + m_ap * (1.0 - alpha);
m_ap = m_at;

m_alp = m_alt;
m_alt = (m_wt - m_wp) / dt;
m_al = m_alt * alpha + m_alp * (1.0 - alpha);

m_w = m_wt * alpha + m_wp * (1.0 - alpha);

quatd wa(m_w.x, m_w.y, m_w.z, 0.0);
quatd wt(m_wt.x, m_wt.y, m_wt.z, 0.0);

m_qt = m_qp + (wt * m_qp) * (0.5 * dt); m_qt.MakeUnit();
m_q = m_qp + (wa * m_qp) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_wp = m_wt;

// output quantities
quatd qi = m_qt.Inverse();
m_Wt = qi * m_wt;
m_rt = m_qt.GetRotationVector();
quatd qi = m_q.Inverse();
m_A = qi * m_a;

FEBodyForce::PrepStep();
}
Expand All @@ -91,24 +134,18 @@ vec3d FEFluidMovingFrameLoad::force(FEMaterialPoint& pt)
{
FEFluidMaterialPoint& fp = *pt.ExtractData<FEFluidMaterialPoint>();

quatd qi = m_q.Inverse();
vec3d a = m_a;
vec3d w = m_w;
vec3d W = qi * w;
vec3d r = pt.m_r0;
vec3d V = fp.m_vft;
vec3d al = qi * m_al;
vec3d f = qi * a + (al ^ r) + (W ^ (W ^ r)) + (W ^ V) * 2.0;

vec3d f = m_A + (m_Al ^ r) + (m_W ^ (m_W ^ r)) + (m_W ^ V) * 2.0;
return f;
}

mat3d FEFluidMovingFrameLoad::stiffness(FEMaterialPoint& pt)
{
const FETimeInfo& tp = GetTimeInfo();

quatd qi = m_q.Inverse();
vec3d W = qi * m_w;
mat3da Sw(W);
mat3da Sw(m_W);
mat3d K = Sw* (2.0 * tp.alphaf);

return K;
Expand Down
14 changes: 8 additions & 6 deletions FEBioFluid/FEFluidMovingFrameLoad.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,17 +42,19 @@ class FEFluidMovingFrameLoad : public FEBodyForce

mat3d stiffness(FEMaterialPoint& pt) override;

public:
vec3d m_at;
vec3d m_wt;
private:
vec3d m_at; // linear acceleration of moving frame in fixed frame
vec3d m_wi; // angular velocity input
int m_frame; // fixed (=0), or moving (=1) frame for input angular velocity

private:
vec3d m_wp, m_w;
vec3d m_ap, m_a;
vec3d m_alt, m_alp, m_al;
vec3d m_ap, m_a, m_A;
vec3d m_wp, m_w, m_Wp, m_W;
vec3d m_alt, m_alp, m_al, m_Alt, m_Alp, m_Al;
quatd m_qt, m_qp, m_q;

// output parameters
vec3d m_wt; // angular velocity in fixed frame (output)
vec3d m_Wt; // angular velocity in moving frame (output)
vec3d m_rt; // rotational vector in fixed frame (output)

Expand Down
111 changes: 72 additions & 39 deletions FEBioMech/FEMovingFrameLoad.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,12 +29,13 @@ SOFTWARE.*/
#include <FECore/log.h>

BEGIN_FECORE_CLASS(FEMovingFrameLoad, FEBodyForce)
ADD_PARAMETER(m_wt.x, "wx")->setLongName("x-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_wt.y, "wy")->setLongName("y-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_wt.z, "wz")->setLongName("z-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_wi.x, "wx")->setLongName("x-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_wi.y, "wy")->setLongName("y-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_wi.z, "wz")->setLongName("z-angular velocity")->setUnits(UNIT_ANGULAR_VELOCITY);
ADD_PARAMETER(m_at.x, "ax")->setLongName("x-linear acceleration")->setUnits(UNIT_ACCELERATION);
ADD_PARAMETER(m_at.y, "ay")->setLongName("y-linear acceleration")->setUnits(UNIT_ACCELERATION);
ADD_PARAMETER(m_at.z, "az")->setLongName("z-linear acceleration")->setUnits(UNIT_ACCELERATION);
ADD_PARAMETER(m_frame, "frame")->setEnums("fixed\0moving\0");

ADD_PARAMETER(m_wt, "wt")->setLongName("Angular velocity in fixed frame")->setUnits(UNIT_ANGULAR_VELOCITY)->SetFlags(FE_PARAM_HIDDEN);
ADD_PARAMETER(m_Wt, "Wt")->setLongName("Angular velocity in moving frame")->setUnits(UNIT_ANGULAR_VELOCITY)->SetFlags(FE_PARAM_HIDDEN);
Expand All @@ -43,13 +44,16 @@ END_FECORE_CLASS();

FEMovingFrameLoad::FEMovingFrameLoad(FEModel* fem) : FEBodyForce(fem)
{
m_frame = 0; // assume fixed frame angular velocity
}

void FEMovingFrameLoad::Activate()
{
m_wp = m_wt;
m_ap = m_at;
m_wp = m_wt;
m_Wp = m_Wt;
m_alp = m_alt;
m_Alp = m_Alt;
m_qp = m_qt;
FEBodyForce::Activate();
}
Expand All @@ -61,33 +65,70 @@ void FEMovingFrameLoad::PrepStep()
double alpha = tp.alphaf;

// evaluate quantities at current time (t) and intermediate time
if (m_frame == 0)
{
m_wt = m_wi; // input is ang vel in fixed frame

// current quantities
m_alp = m_alt;
m_alt = (m_wt - m_wp) / dt;
quatd wt(m_wt.x, m_wt.y, m_wt.z, 0.0);
m_qt = m_qp + (wt * m_qp) * (0.5 * dt); m_qt.MakeUnit();

// intermediate quantities
m_al = m_alt * alpha + m_alp * (1.0 - alpha);
m_w = m_wt * alpha + m_wp * (1.0 - alpha);
quatd wa(m_w.x, m_w.y, m_w.z, 0.0);
m_q = m_qp + (wa * m_qp) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_wp = m_wt;

// quantities in body frame
quatd qti = m_qt.Inverse();
m_Wt = qti * m_wt;
m_rt = m_qt.GetRotationVector();

quatd qi = m_q.Inverse();
m_Al = qi * m_al;
m_W = qi * m_w;
}
else
{
m_Wt = m_wi; // input is ang vel in body frame

// current quantities
m_Alp = m_Alt;
m_Alt = (m_Wt - m_Wp) / dt;
quatd Wt(m_Wt.x, m_Wt.y, m_Wt.z, 0.0);
m_qt = m_qp + (m_qp * Wt) * (0.5 * dt); m_qt.MakeUnit();

// intermediate quantities
m_Al = m_Alt * alpha + m_Alp * (1.0 - alpha);
m_W = m_Wt * alpha + m_Wp * (1.0 - alpha);
quatd Wa(m_W.x, m_W.y, m_W.z, 0.0);
m_q = m_qp + (m_qp * Wa) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_Wp = m_Wt;

// quantities in fixed frame
m_wt = m_qt * m_Wt;
m_rt = m_qt.GetRotationVector();
m_alp = m_alt;
m_alt = m_qt * m_Alt;

m_al = m_q * m_Al;
m_w = m_q * m_W;
}

// linear acceleration (always assumed to be given in the fixed frame)
m_a = m_at * alpha + m_ap * (1.0 - alpha);
m_ap = m_at;
quatd qi = m_q.Inverse();
m_A = qi * m_a;

m_alp = m_alt;
m_alt = (m_wt - m_wp) / dt;
m_al = m_alt * alpha + m_alp * (1.0 - alpha);

m_w = m_wt * alpha + m_wp * (1.0 - alpha);

quatd wa(m_w.x, m_w.y, m_w.z, 0.0);
quatd wt(m_wt.x, m_wt.y, m_wt.z, 0.0);

m_qt = m_qp + (wt * m_qp) * (0.5 * dt); m_qt.MakeUnit();
m_q = m_qp + (wa * m_qp) * (0.5 * dt * alpha); m_q.MakeUnit();

m_qp = m_qt;
m_wp = m_wt;

// output quantities
quatd qi = m_qt.Inverse();
m_Wt = qi * m_wt;
m_rt = m_qt.GetRotationVector();

vec3d R = m_q.GetRotationVector();

feLogDebug("al : %lg, %lg, %lg\n", m_al.x, m_al.y, m_al.z);
feLogDebug("R : %lg, %lg, %lg\n", R.x, R.y, R.z);
FEBodyForce::PrepStep();
}

vec3d FEMovingFrameLoad::force(FEMaterialPoint& pt)
Expand All @@ -101,11 +142,7 @@ vec3d FEMovingFrameLoad::force(FEMaterialPoint& pt)
vec3d r = pt.m_rt;
vec3d v = ep.m_v;

quatd qi = m_q.Inverse();
vec3d w = qi * m_w;
vec3d al = qi * m_al;

vec3d f = qi*m_a + (al ^ r) + (w ^ (w ^ r)) + (w ^ v) * 2.0;
vec3d f = m_A + (m_Al ^ r) + (m_W ^ (m_W ^ r)) + (m_W ^ v) * 2.0;
return f;
}

Expand All @@ -119,12 +156,8 @@ mat3d FEMovingFrameLoad::stiffness(FEMaterialPoint& pt)
double dt = tp.timeIncrement;
double alpha = tp.alphaf;

quatd qi = m_q.Inverse();
vec3d w = qi * m_w;
vec3d al = qi * m_al;

mat3da Sw(w);
mat3da A(al);
mat3da Sw(m_W);
mat3da A(m_Al);
mat3d S2 = Sw * Sw;
mat3d K = (S2 + A + Sw * (4.0 * gamma / dt))*alpha;

Expand Down
15 changes: 9 additions & 6 deletions FEBioMech/FEMovingFrameLoad.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,15 +41,18 @@ class FEMovingFrameLoad : public FEBodyForce
mat3d stiffness(FEMaterialPoint& pt) override;

private:
vec3d m_wt; // angular velocity of frame
vec3d m_at; // linear acceleration of frame

vec3d m_wp, m_w;
vec3d m_ap, m_a;
vec3d m_alt, m_alp, m_al;
vec3d m_at; // linear acceleration of moving frame in fixed frame
vec3d m_wi; // angular velocity input
int m_frame; // fixed (=0), or moving (=1) frame for input angular velocity

private:
vec3d m_ap, m_a, m_A;
vec3d m_wp, m_w, m_Wp, m_W;
vec3d m_alt, m_alp, m_al, m_Alt, m_Alp, m_Al;
quatd m_qt, m_qp, m_q;

// output parameters
vec3d m_wt; // angular velocity in fixed frame (output)
vec3d m_Wt; // angular velocity in moving frame (output)
vec3d m_rt; // rotational vector in fixed frame (output)

Expand Down

0 comments on commit a7cb50b

Please sign in to comment.