29 if (INI_FILE !=
nullptr)
initialize(*INI_FILE);
34 options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
38 options.saveToConfigFile(c, getConfigFileSectionName());
46 const auto ptg = getAssociatedPTG();
47 const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
50 no.
logRecord = std::make_shared<CLogFileRecord_VFF>();
58 const double inc_ang = 2 *
M_PI / n;
59 double ang = -
M_PI + 0.5 * inc_ang;
60 for (
size_t i = 0; i < n; i++, ang += inc_ang)
64 const double mod = std::min(1e6, 1.0 / ni.
obstacles[i]);
67 instantaneousForce.
x = -cos(ang) * mod;
68 instantaneousForce.
y = -sin(ang) * mod;
69 resultantForce += instantaneousForce;
73 const double obstcl_weight = 20.0 / ni.
obstacles.size();
74 resultantForce *= obstcl_weight;
76 const double obstacleNearnessFactor =
77 std::min(1.0, 6.0 / resultantForce.norm());
81 const auto trg = *ni.
targets.rbegin();
83 const double ang = atan2(trg.y, trg.x);
84 const double mod = options.TARGET_ATTRACTIVE_FORCE;
90 : atan2(resultantForce.y, resultantForce.x);
94 if (m_enableApproachTargetSlowDown)
96 const double targetNearnessFactor = std::min(
98 (options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist));
101 std::min(obstacleNearnessFactor, targetNearnessFactor);
105 uint8_t CHolonomicVFF::serializeGetVersion()
const {
return 0; }
108 out << options.TARGET_ATTRACTIVE_FORCE
109 << options.TARGET_SLOW_APPROACHING_DISTANCE;
111 void CHolonomicVFF::serializeFrom(
118 in >> options.TARGET_ATTRACTIVE_FORCE >>
119 options.TARGET_SLOW_APPROACHING_DISTANCE;
127 uint8_t CLogFileRecord_VFF::serializeGetVersion()
const {
return 0; }
132 void CLogFileRecord_VFF::serializeFrom(
149 CHolonomicVFF::TOptions::TOptions()
153 void CHolonomicVFF::TOptions::loadFromConfigFile(
160 TARGET_SLOW_APPROACHING_DISTANCE,
double, source, section);
166 void CHolonomicVFF::TOptions::saveToConfigFile(
172 TARGET_SLOW_APPROACHING_DISTANCE,
"For stopping gradually");
174 TARGET_ATTRACTIVE_FORCE,
175 "Dimension-less (may have to be tuned depending on the density of "
176 "obstacle sampling)");