Skip to content

Commit 112391e

Browse files
tobbelobbtobbelobb
committed
Early return
1 parent 8e146d8 commit 112391e

File tree

1 file changed

+97
-95
lines changed

1 file changed

+97
-95
lines changed

‎src/Movement/Kinematics/HangprinterKinematics.cpp‎

Lines changed: 97 additions & 95 deletions
Original file line numberDiff line numberDiff line change
@@ -1167,118 +1167,120 @@ void HangprinterKinematics::StaticForcesQuadrilateralPyramid(float const machine
11671167
// The forces in the top anchor is assumed to be known and constant, except for gravity's
11681168
// effects who are also known.
11691169
if (moverWeight_kg < 0.0001) {
1170-
// Space for four linear 3x3 systems, each with two solution columns,
1171-
FixedMatrix<float, 3, 5> M[4];
1172-
1173-
float norm[5];
1174-
norm[4] = hyp3(anchors[4], machinePos);
1175-
for (int i = 0; i < 4; ++i) {
1176-
norm[i] = hyp3(anchors[i], machinePos);
1177-
for (int j = 0; j < 3; ++j) {
1178-
for (int k = 0; k < 4; ++k) {
1179-
// Fill 3x3 top left corner of system with
1180-
// unit vectors toward each ABCD anchor from mover
1181-
// If A is the column vector pointing towards A-anchor, we're building these
1182-
// four matrices:
1183-
// k=0: [BCD], A-direction skipped
1184-
// k=1: [ACD], B-direction skipped
1185-
// k=2: [ABD], C-direction skipped
1186-
// k=3: [ABC], D-direction skipped
1187-
if ( k != i) {
1188-
if ( i > k ) {
1189-
M[k](j, i - 1) = (anchors[i][j] - machinePos[j]) / norm[i];
1190-
} else {
1191-
M[k](j, i) = (anchors[i][j] - machinePos[j]) / norm[i];
1192-
}
1170+
return;
1171+
}
1172+
// Space for four linear 3x3 systems, each with two solution columns,
1173+
FixedMatrix<float, 3, 5> M[4];
1174+
1175+
float norm[5];
1176+
norm[4] = hyp3(anchors[4], machinePos);
1177+
for (int i = 0; i < 4; ++i) {
1178+
norm[i] = hyp3(anchors[i], machinePos);
1179+
for (int j = 0; j < 3; ++j) {
1180+
for (int k = 0; k < 4; ++k) {
1181+
// Fill 3x3 top left corner of system with
1182+
// unit vectors toward each ABCD anchor from mover
1183+
// If A is the column vector pointing towards A-anchor, we're building these
1184+
// four matrices:
1185+
// k=0: [BCD], A-direction skipped
1186+
// k=1: [ACD], B-direction skipped
1187+
// k=2: [ABD], C-direction skipped
1188+
// k=3: [ABC], D-direction skipped
1189+
if ( k != i) {
1190+
if ( i > k ) {
1191+
M[k](j, i - 1) = (anchors[i][j] - machinePos[j]) / norm[i];
1192+
} else {
1193+
M[k](j, i) = (anchors[i][j] - machinePos[j]) / norm[i];
11931194
}
11941195
}
11951196
}
11961197
}
1197-
float const mg = moverWeight_kg * 9.81;
1198+
}
1199+
float const mg = moverWeight_kg * 9.81;
11981200

1199-
float top_mg = 0.0F;
1200-
float top_pre = 0.0F;
1201+
float top_mg = 0.0F;
1202+
float top_pre = 0.0F;
12011203

1202-
if (anchors[4][Z_AXIS] > machinePos[Z_AXIS]) {
1203-
// These force constants will go into the solution column that has to do with gravity
1204-
top_mg = mg / ((anchors[4][Z_AXIS] - machinePos[Z_AXIS]) / norm[4]);
1205-
top_pre = targetForce_Newton;
1206-
}
1204+
if (anchors[4][Z_AXIS] > machinePos[Z_AXIS]) {
1205+
// These force constants will go into the solution column that has to do with gravity
1206+
top_mg = mg / ((anchors[4][Z_AXIS] - machinePos[Z_AXIS]) / norm[4]);
1207+
top_pre = targetForce_Newton;
1208+
}
12071209

1208-
// Indices for the two solution columns
1209-
size_t const sol_mg = 3;
1210-
size_t const sol_pt = 4;
1211-
for (int i = 0; i < 3; ++i) {
1212-
float const top_dist = (anchors[4][i] - machinePos[i]) / norm[4];
1213-
for (int k = 0; k < 4; ++k) {
1214-
M[k](i, sol_mg) = -top_mg * top_dist; // gravity solution column
1215-
M[k](i, sol_pt) = -top_pre * top_dist; // pretension solution column
1216-
}
1217-
}
1210+
// Indices for the two solution columns
1211+
size_t const sol_mg = 3;
1212+
size_t const sol_pt = 4;
1213+
for (int i = 0; i < 3; ++i) {
1214+
float const top_dist = (anchors[4][i] - machinePos[i]) / norm[4];
12181215
for (int k = 0; k < 4; ++k) {
1219-
// Cancel out top anchor's Z-force with gravity.
1220-
M[k](Z_AXIS, sol_mg) += mg; // == 0
1216+
M[k](i, sol_mg) = -top_mg * top_dist; // gravity solution column
1217+
M[k](i, sol_pt) = -top_pre * top_dist; // pretension solution column
12211218
}
1219+
}
1220+
for (int k = 0; k < 4; ++k) {
1221+
// Cancel out top anchor's Z-force with gravity.
1222+
M[k](Z_AXIS, sol_mg) += mg; // == 0
1223+
}
12221224

1223-
// Solve the four systems
1224-
for (int k = 0; k < 4; ++k) {
1225-
M[k].GaussJordan(3, 5);
1226-
}
1225+
// Solve the four systems
1226+
for (int k = 0; k < 4; ++k) {
1227+
M[k].GaussJordan(3, 5);
1228+
}
12271229

1228-
// Weigh/scale the pre-tension solutions so all have equal max force.
1229-
float norm_ABCD[4];
1230-
for(size_t k{0}; k < 4; ++k) {
1231-
norm_ABCD[k] = fastSqrtf(M[k](0, sol_pt) * M[k](0, sol_pt) + M[k](1, sol_pt) * M[k](1, sol_pt) + M[k](2, sol_pt) * M[k](2, sol_pt));
1232-
}
1230+
// Weigh/scale the pre-tension solutions so all have equal max force.
1231+
float norm_ABCD[4];
1232+
for(size_t k{0}; k < 4; ++k) {
1233+
norm_ABCD[k] = fastSqrtf(M[k](0, sol_pt) * M[k](0, sol_pt) + M[k](1, sol_pt) * M[k](1, sol_pt) + M[k](2, sol_pt) * M[k](2, sol_pt));
1234+
}
12331235

1234-
// Arrays to hold our weighted combinations of the four (pairs of) solutions
1235-
float p[4] = { 0.0F, 0.0F, 0.0F, 0.0F };
1236-
float m[4] = { 0.0F, 0.0F, 0.0F, 0.0F };
1237-
for (size_t i{0}; i < 3; ++i) {
1238-
for (size_t j{0}; j < 4; ++j) {
1239-
float const pt_weight = targetForce_Newton / norm_ABCD[j];
1240-
// The gravity counter actions are scaled to exactly counter act gravity, and top-line forces neccesary to counter act gravity.
1241-
// So the resultant force of all four solutions is the same. Lets add a quarter of each solution to get back that resultant force.
1242-
float const mg_weight = 1.0/4.0;
1243-
// i can mean BCD, ACD, ABD, or ABC, depending on which matrix we're looking into
1244-
// Let's just translate that back into the solutions vectors
1245-
size_t const s = j <= i ? i + 1 : i;
1246-
p[s] += M[j](i, sol_pt)*pt_weight;
1247-
m[s] += M[j](i, sol_mg)*mg_weight;
1248-
}
1236+
// Arrays to hold our weighted combinations of the four (pairs of) solutions
1237+
float p[4] = { 0.0F, 0.0F, 0.0F, 0.0F };
1238+
float m[4] = { 0.0F, 0.0F, 0.0F, 0.0F };
1239+
for (size_t i{0}; i < 3; ++i) {
1240+
for (size_t j{0}; j < 4; ++j) {
1241+
float const pt_weight = targetForce_Newton / norm_ABCD[j];
1242+
// The gravity counter actions are scaled to exactly counter act gravity, and top-line forces neccesary to counter act gravity.
1243+
// So the resultant force of all four solutions is the same. Lets add a quarter of each solution to get back that resultant force.
1244+
float const mg_weight = 1.0/4.0;
1245+
// i can mean BCD, ACD, ABD, or ABC, depending on which matrix we're looking into
1246+
// Let's just translate that back into the solutions vectors
1247+
size_t const s = j <= i ? i + 1 : i;
1248+
p[s] += M[j](i, sol_pt)*pt_weight;
1249+
m[s] += M[j](i, sol_mg)*mg_weight;
12491250
}
1251+
}
12501252

1251-
// The pre-tension solution can be scaled up or down however we want.
1252-
// Forces in those solution cancel each other out exactly, so any multiple of the solution is also a valid solution.
1253-
//
1254-
// (The gravity solution can't be scaled since it has to exactly counter act top-line forces that must exactly counter act gravity (mg))
1255-
//
1256-
// Use the scaling freedom of the pre-tension solution to assure that we have at least targetForce_Newton in the ABCD lines,
1257-
// and that no line (incl top-line) get more tension than the configured maxPlannedForce in that direction.
1258-
float preFac = min(max(std::abs((targetForce_Newton - m[3]) / p[3]),
1259-
max(std::abs((targetForce_Newton - m[2]) / p[2]),
1260-
max(std::abs((targetForce_Newton - m[1]) / p[1]), std::abs((targetForce_Newton - m[0]) / p[0])))),
1261-
min(std::abs((maxPlannedForce_Newton[4] - top_mg) / top_pre),
1262-
min(min(std::abs((maxPlannedForce_Newton[0] - m[0]) / p[0]), std::abs((maxPlannedForce_Newton[1] - m[1]) / p[1])),
1263-
min(std::abs((maxPlannedForce_Newton[2] - m[2]) / p[2]), std::abs((maxPlannedForce_Newton[3] - m[3]) / p[3])))));
1264-
1265-
float tot[5] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F };
1266-
tot[0] = m[0] + preFac * p[0];
1267-
tot[1] = m[1] + preFac * p[1];
1268-
tot[2] = m[2] + preFac * p[2];
1269-
tot[3] = m[3] + preFac * p[3];
1270-
tot[4] = top_mg + preFac * top_pre;
1271-
1272-
for (size_t i{0}; i < 5; ++i) {
1273-
// Negative, or very large forces can still have slipped through the preFac filter.
1274-
// Truncate away such forces and assign to the output variable.
1275-
// Voila.
1276-
// The min( ... ) shouldn't be needed here. Just better safe than sorry.
1277-
F[i] = min(max(tot[i], minPlannedForce_Newton[i]), maxPlannedForce_Newton[i]);
1278-
}
1253+
// The pre-tension solution can be scaled up or down however we want.
1254+
// Forces in those solution cancel each other out exactly, so any multiple of the solution is also a valid solution.
1255+
//
1256+
// (The gravity solution can't be scaled since it has to exactly counter act top-line forces that must exactly counter act gravity (mg))
1257+
//
1258+
// Use the scaling freedom of the pre-tension solution to assure that we have at least targetForce_Newton in the ABCD lines,
1259+
// and that no line (incl top-line) get more tension than the configured maxPlannedForce in that direction.
1260+
float preFac = min(max(std::abs((targetForce_Newton - m[3]) / p[3]),
1261+
max(std::abs((targetForce_Newton - m[2]) / p[2]),
1262+
max(std::abs((targetForce_Newton - m[1]) / p[1]), std::abs((targetForce_Newton - m[0]) / p[0])))),
1263+
min(std::abs((maxPlannedForce_Newton[4] - top_mg) / top_pre),
1264+
min(min(std::abs((maxPlannedForce_Newton[0] - m[0]) / p[0]), std::abs((maxPlannedForce_Newton[1] - m[1]) / p[1])),
1265+
min(std::abs((maxPlannedForce_Newton[2] - m[2]) / p[2]), std::abs((maxPlannedForce_Newton[3] - m[3]) / p[3])))));
1266+
1267+
float tot[5] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F };
1268+
tot[0] = m[0] + preFac * p[0];
1269+
tot[1] = m[1] + preFac * p[1];
1270+
tot[2] = m[2] + preFac * p[2];
1271+
tot[3] = m[3] + preFac * p[3];
1272+
tot[4] = top_mg + preFac * top_pre;
1273+
1274+
for (size_t i{0}; i < 5; ++i) {
1275+
// Negative, or very large forces can still have slipped through the preFac filter.
1276+
// Truncate away such forces and assign to the output variable.
1277+
// Voila.
1278+
// The min( ... ) shouldn't be needed here. Just better safe than sorry.
1279+
F[i] = min(max(tot[i], minPlannedForce_Newton[i]), maxPlannedForce_Newton[i]);
12791280
}
12801281
}
12811282

1283+
12821284
void HangprinterKinematics::StaticForcesTetrahedron(float const machinePos[3], float F[HANGPRINTER_MAX_ANCHORS]) const noexcept {
12831285
static constexpr size_t A_AXIS = 0;
12841286
static constexpr size_t B_AXIS = 1;

0 commit comments

Comments
 (0)