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); } }