SegmentList<DataFacadeT>::SegmentList(const InternalRouteResult &raw_route,
const bool extract_alternative,
const unsigned zoom_level,
const bool allow_simplification,
const DataFacade *facade)
: total_distance(0), total_duration(0)
{
if (!raw_route.is_valid())
{
return;
}
if (extract_alternative)
{
BOOST_ASSERT(raw_route.has_alternative());
InitRoute(raw_route.segment_end_coordinates.front().source_phantom,
raw_route.alt_source_traversed_in_reverse.front());
AddLeg(raw_route.unpacked_alternative,
raw_route.segment_end_coordinates.back().target_phantom,
raw_route.alt_source_traversed_in_reverse.back(), false, facade);
}
else
{
InitRoute(raw_route.segment_end_coordinates.front().source_phantom,
raw_route.source_traversed_in_reverse.front());
for (std::size_t raw_index = 0; raw_index < raw_route.segment_end_coordinates.size();
++raw_index)
{
AddLeg(raw_route.unpacked_path_segments[raw_index],
raw_route.segment_end_coordinates[raw_index].target_phantom,
raw_route.target_traversed_in_reverse[raw_index],
raw_route.is_via_leg(raw_index), facade);
if (raw_route.is_via_leg(raw_index))
{
const auto &source_phantom =
raw_route.segment_end_coordinates[raw_index].target_phantom;
if (raw_route.target_traversed_in_reverse[raw_index] !=
raw_route.source_traversed_in_reverse[raw_index + 1])
{
bool traversed_in_reverse = raw_route.target_traversed_in_reverse[raw_index];
const extractor::TravelMode travel_mode =
(traversed_in_reverse ? source_phantom.backward_travel_mode
: source_phantom.forward_travel_mode);
const bool constexpr IS_NECESSARY = true;
const bool constexpr IS_VIA_LOCATION = true;
segments.emplace_back(source_phantom.location, source_phantom.name_id, 0, 0.f,
extractor::TurnInstruction::UTurn, IS_NECESSARY,
IS_VIA_LOCATION, travel_mode);
}
}
}
}
if (!allow_simplification)
{
// to prevent any simplifications, we mark all segments as necessary
for (auto &segment : segments)
{
segment.necessary = true;
}
}
Finalize(extract_alternative, raw_route, zoom_level, allow_simplification);
}
void MakeHexapod(DemoEntityManager* const scene, const dMatrix& location)
{
dFloat mass = 30.0f;
// make the kinematic solver
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
// make the root body
dMatrix baseMatrix(dGetIdentityMatrix());
baseMatrix.m_posit.m_y += 0.35f;
dVector size (1.3f, 0.31f, 0.5f, 0.0f);
NewtonBody* const hexaBody = CreateBox(scene, baseMatrix * location, size, mass, 1.0f);
void* const hexaBodyNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, hexaBody);
int legEffectorCount = 0;
dCustomInverseDynamicsEffector* legEffectors[32];
baseMatrix.m_posit.m_y -= 0.06f;
// make the hexapod six limbs
for (int i = 0; i < 3; i ++) {
dMatrix rightLocation (baseMatrix);
rightLocation.m_posit += rightLocation.m_right.Scale (size.m_z * 0.65f);
rightLocation.m_posit += rightLocation.m_front.Scale (size.m_x * 0.3f - size.m_x * i / 3.0f);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, rightLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
dMatrix similarTransform (dGetIdentityMatrix());
similarTransform.m_posit.m_x = rightLocation.m_posit.m_x;
similarTransform.m_posit.m_y = rightLocation.m_posit.m_y;
dMatrix leftLocation (rightLocation * similarTransform.Inverse() * dYawMatrix(dPi) * similarTransform);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, leftLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
}
// finalize inverse dynamics solver
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
// create a fix pose frame generator
dEffectorTreeFixPose* const idlePose = new dEffectorTreeFixPose(hexaBody);
dEffectorTreeFixPose* const walkPoseGenerator = new dEffectorWalkPoseGenerator(hexaBody);
m_walkIdleBlender = new dEffectorBlendIdleWalk (hexaBody, idlePose, walkPoseGenerator);
m_postureModifier = new dAnimationHipController(m_walkIdleBlender);
m_animTreeNode = new dEffectorTreeRoot(hexaBody, m_postureModifier);
dMatrix rootMatrix;
NewtonBodyGetMatrix (hexaBody, &rootMatrix[0][0]);
rootMatrix = rootMatrix.Inverse();
for (int i = 0; i < legEffectorCount; i++) {
dEffectorTreeInterface::dEffectorTransform frame;
dCustomInverseDynamicsEffector* const effector = legEffectors[i];
dMatrix effectorMatrix(effector->GetBodyMatrix());
dMatrix poseMatrix(effectorMatrix * rootMatrix);
frame.m_effector = effector;
frame.m_posit = poseMatrix.m_posit;
frame.m_rotation = dQuaternion(poseMatrix);
idlePose->GetPose().Append(frame);
walkPoseGenerator->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
}
}
