Skip to content
Merged
Show file tree
Hide file tree
Changes from all 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