Skip to content
Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .clang-format-ignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
source/Eigen/*
214 changes: 173 additions & 41 deletions CHANGELOG.md

Large diffs are not rendered by default.

81 changes: 47 additions & 34 deletions source/Body.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -80,20 +80,21 @@ Body::setup(int number_in,
bodyI = I_in;
bodyCdA = CdA_in;
bodyCa = Ca_in;
} else if (type == FIXED){ // fixed bodies have no need for these variables other than position...
} else if (type == FIXED) { // fixed bodies have no need for these variables
// other than position...
bodyM = 0.0;
bodyV = 0.0;
bodyI = vec::Zero();
bodyCdA = vec6::Zero();
bodyCa = vec6::Zero();
} else if (type == CPLDPIN){
} else if (type == CPLDPIN) {
bodyM = M_in;
bodyV = V_in;
body_rCG = rCG_in;
bodyI = I_in;
bodyCdA = CdA_in;
bodyCa = Ca_in;
} else // coupled bodies have no need for these variables...
} else // coupled bodies have no need for these variables...
{
bodyM = 0.0;
bodyV = 0.0;
Expand Down Expand Up @@ -203,9 +204,10 @@ Body::initialize()
// initialized)
setDependentStates();

if (type == FREE) { // Attached objects already initalized for coupled pinned body
// If any Rod is fixed to the body (not pinned), initialize it now because
// otherwise it won't be initialized
if (type ==
FREE) { // Attached objects already initalized for coupled pinned body
// If any Rod is fixed to the body (not pinned), initialize it now
// because otherwise it won't be initialized
for (auto attached : attachedR)
if (attached->type == Rod::FIXED)
attached->initialize();
Expand Down Expand Up @@ -273,7 +275,6 @@ Body::setDependentStates()
rPoint,
rdPoint); //<<< should double check this function


// pass above to the point and get it to calculate the forces
try {
attachedP[i]->setKinematics(rPoint, rdPoint);
Expand All @@ -294,12 +295,8 @@ Body::setDependentStates()
// do 3d details of Rod ref point
vec tmpr, tmprd;
// set first three entires (end A translation) of rRod and rdRod
transformKinematics(r6RodRel[i](Eigen::seqN(0, 3)),
OrMat,
r7.pos,
v6,
tmpr,
tmprd);
transformKinematics(
r6RodRel[i](Eigen::seqN(0, 3)), OrMat, r7.pos, v6, tmpr, tmprd);
// does the above function need to take in all 6 elements of r6RodRel??
rRod(Eigen::seqN(0, 3)) = tmpr;
rdRod(Eigen::seqN(0, 3)) = tmprd;
Expand Down Expand Up @@ -331,9 +328,11 @@ Body::GetBodyOutput(OutChanProps outChan)
vec3 rotations;
vec6 Fout;

if ((outChan.QType == RX)||(outChan.QType == RY)||(outChan.QType == RZ))
if ((outChan.QType == RX) || (outChan.QType == RY) || (outChan.QType == RZ))
rotations = rad2deg * Quat2Euler(r7.quat);
if ((outChan.QType == FX)||(outChan.QType == FY)||(outChan.QType == FZ)||(outChan.QType == MX)||(outChan.QType == MY)||(outChan.QType == MZ))
if ((outChan.QType == FX) || (outChan.QType == FY) ||
(outChan.QType == FZ) || (outChan.QType == MX) ||
(outChan.QType == MY) || (outChan.QType == MZ))
Fout = getFnet();

if (outChan.QType == PosX)
Expand Down Expand Up @@ -414,8 +413,10 @@ Body::initiateStep(vec6 r, vec6 rd, vec6 rdd)
}
if (type == FIXED) // if fixed body, set the BCs to stationary
{
if (bodyId == 0) r_ves = vec6::Zero(); // special ground body case
else r_ves = r;
if (bodyId == 0)
r_ves = vec6::Zero(); // special ground body case
else
r_ves = r;
rd_ves = vec6::Zero();
rdd_ves = vec6::Zero();
return;
Expand Down Expand Up @@ -493,19 +494,24 @@ Body::getStateDeriv(InstanceStateVarView drdt)
// Get contributions from attached points (and lines attached to
// them) and store in FNet:
doRHS();
if (type == FREE){
if (type == FREE) {
// solve for accelerations in [M]{a}={f}
a6 = solveMat6(M, F6net);

// NOTE; is the above still valid even though it includes rotational DOFs?
// NOTE; is the above still valid even though it includes rotational
// DOFs?
u7.pos = v6.head<3>();
// this assumes that the angular velocity is about the global coordinates
// which is true for bodies
u7.quat = 0.5 * (quaternion(0.0, v6[3], v6[4], v6[5]) * r7.quat).coeffs();
// this assumes that the angular velocity is about the global
// coordinates which is true for bodies
u7.quat =
0.5 * (quaternion(0.0, v6[3], v6[4], v6[5]) * r7.quat).coeffs();
} else { // Pinned body

// account for moment in response to acceleration due to inertial coupling (off-diagonal sub-matrix terms)
const vec Fnet_out3 = F6net(Eigen::seqN(3, 3)) - (M.bottomLeftCorner<3,3>() * a6(Eigen::seqN(0, 3)));
// account for moment in response to acceleration due to inertial
// coupling (off-diagonal sub-matrix terms)
const vec Fnet_out3 =
F6net(Eigen::seqN(3, 3)) -
(M.bottomLeftCorner<3, 3>() * a6(Eigen::seqN(0, 3)));

// For small systems it is usually faster to compute the inverse
// of the matrix. See
Expand All @@ -515,7 +521,8 @@ Body::getStateDeriv(InstanceStateVarView drdt)

// dxdt = V (velocities)
u7.pos = vec::Zero();
u7.quat = 0.5 * (quaternion(0.0, v6[3], v6[4], v6[5]) * r7.quat).coeffs();
u7.quat =
0.5 * (quaternion(0.0, v6[3], v6[4], v6[5]) * r7.quat).coeffs();
}

drdt.row(0).head<7>() = u7.toVec7();
Expand All @@ -530,12 +537,15 @@ Body::getFnet() const

// this assumes doRHS() has already been called
if (type == COUPLED) { // if coupled rigidly
F6_iner = - M * a6; // Inertial terms
F6_iner = -M * a6; // Inertial terms
Fnet_out = F6net + F6_iner;
} else if (type == CPLDPIN) { // if coupled pinned
F6_iner(Eigen::seqN(0,3)) = (- M.topLeftCorner<3,3>() * a6(Eigen::seqN(0,3))) - (M.topRightCorner<3,3>() * a6(Eigen::seqN(3,3))); // Inertial term
Fnet_out(Eigen::seqN(0,3)) = F6net(Eigen::seqN(0,3)) + F6_iner(Eigen::seqN(0,3));
Fnet_out(Eigen::seqN(3,3)) = vec3::Zero();
F6_iner(Eigen::seqN(0, 3)) =
(-M.topLeftCorner<3, 3>() * a6(Eigen::seqN(0, 3))) -
(M.topRightCorner<3, 3>() * a6(Eigen::seqN(3, 3))); // Inertial term
Fnet_out(Eigen::seqN(0, 3)) =
F6net(Eigen::seqN(0, 3)) + F6_iner(Eigen::seqN(0, 3));
Fnet_out(Eigen::seqN(3, 3)) = vec3::Zero();
} else {
// LOGERR << "Invalid body type: " << TypeName(type) << endl;
// throw moordyn::invalid_value_error("Invalid body type");
Expand Down Expand Up @@ -573,7 +583,7 @@ Body::doRHS()
// Centrifugal force due to COM not being at body origin
const vec w = v6.tail<3>();
F6net.head<3>() -=
M.topLeftCorner(3, 3) * (w.cross(w.cross(body_rCGrotated)));
M.topLeftCorner(3, 3) * (w.cross(w.cross(body_rCGrotated)));

// --------------------------------- apply wave kinematics
// ------------------------------------
Expand All @@ -594,10 +604,13 @@ Body::doRHS()

// Rotational DOFs drag coefficients are also defined on bodyCdA
vec6 cda;
cda(Eigen::seqN(0, 3)) = (OrMat * bodyCdA.head<3>().asDiagonal() * OrMat.transpose()) * vi.head<3>() * vi.head<3>().norm();
cda(Eigen::seqN(3, 3)) = (OrMat * bodyCdA.tail<3>().asDiagonal() * OrMat.transpose()) * vi.tail<3>() * vi.tail<3>().norm();
F6net +=
0.5 * env->rho_w * cda;
cda(Eigen::seqN(0, 3)) =
(OrMat * bodyCdA.head<3>().asDiagonal() * OrMat.transpose()) *
vi.head<3>() * vi.head<3>().norm();
cda(Eigen::seqN(3, 3)) =
(OrMat * bodyCdA.tail<3>().asDiagonal() * OrMat.transpose()) *
vi.tail<3>() * vi.tail<3>().norm();
F6net += 0.5 * env->rho_w * cda;

// Get contributions from any points attached to the body
for (auto attached : attachedP) {
Expand Down
Loading
Loading