Vector<SimpleTransform> NavigationGraph::createTransformsFromPath(const Vector<NavigationNode*>& path, float scale)
{
if (path.size() < 2)
return {};
auto transforms = Vector<SimpleTransform>();
auto direction = Vec3();
for (auto i = 0U; i < path.size(); i++)
{
if (i + 1 < path.size())
direction = (path[i + 1]->getPosition() - path[i]->getPosition()).normalized();
auto v0 = Vec3();
auto v1 = Vec3();
direction.constructBasis(v0, v1);
auto orientation = Quaternion::createFromRotationMatrix(
{v0.x, v1.x, direction.x, v0.y, v1.y, direction.y, v0.z, v1.z, direction.z});
// Move to the new position
transforms.emplace(path[i]->getPosition() * scale,
transforms.size() ? transforms.back().getOrientation() : orientation);
// Rotate on the spot if needed
if (i > 0 && transforms.back().getOrientation().getZVector().dot(direction) < 1.0f - Math::Epsilon)
transforms.emplace(transforms.back().getPosition(), orientation);
}
return transforms;
}
void CylinderVectorGenerator::destination(const Vector3 & destination)
{
direction_ = destination - origin_;
constructBasis();
}
