281 lines
8.1 KiB
C++
281 lines
8.1 KiB
C++
////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Paella
|
|
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
|
|
// Authors :
|
|
// Thomas FORGIONE
|
|
// Emilie JALRAS
|
|
// Marion LENFANT
|
|
// Thierry MALON
|
|
// Amandine PAILLOUX
|
|
//
|
|
// This file is part of the project Paella
|
|
// This software is provided 'as-is', without any express or implied warranty.
|
|
// In no event will the authors be held liable for any damages arising from the use of this software.
|
|
//
|
|
// Permission is granted to anyone to use this software for any purpose,
|
|
// including commercial applications, and to alter it and redistribute it freely,
|
|
// subject to the following restrictions:
|
|
//
|
|
// 1. The origin of this software must not be misrepresented;
|
|
// you must not claim that you wrote the original software.
|
|
// If you use this software in a product, an acknowledgment
|
|
// in the product documentation would be appreciated but is not required.
|
|
//
|
|
// 2. Altered source versions must be plainly marked as such,
|
|
// and must not be misrepresented as being the original software.
|
|
//
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <cmath>
|
|
#include "Geometry/Vector.hpp"
|
|
#include "Geometry/Base.hpp"
|
|
#include "Geometry/Spline.hpp"
|
|
|
|
namespace geo
|
|
{
|
|
|
|
template<typename... Types>
|
|
Spline<Types...>::Spline() : controlPoints{}, nodes{}, degree{}
|
|
{
|
|
|
|
}
|
|
|
|
template<typename... Types>
|
|
std::vector<Circle<float>> Spline<Types...>::computeCircles(unsigned int nbCircles, unsigned int const nbPoints, unsigned int const globalOffset) const
|
|
{
|
|
std::vector<Circle<float>> circles;
|
|
unsigned int offset = 0;
|
|
|
|
Vector3<float> v{0.0f,0.0f,1.0f};
|
|
|
|
for (unsigned int i = 0; i < nbCircles; i++)
|
|
{
|
|
auto circle = computeCircle(static_cast<float>(i)/(nbCircles-1), nbPoints, v, globalOffset+offset);
|
|
if (!circle.points.empty())
|
|
{
|
|
circles.push_back(circle);
|
|
offset += nbPoints;
|
|
}
|
|
}
|
|
|
|
return circles;
|
|
|
|
}
|
|
|
|
template<typename... Types>
|
|
Circle<float> Spline<Types...>::computeCircle(float t, unsigned int const nbPoints, Vector3<float>& v, unsigned int const offset) const
|
|
{
|
|
Circle<float> circlePoints;
|
|
|
|
auto S_t = (*this)(t);
|
|
auto S_prime_t = prime(t);
|
|
auto C_t = std::get<0>(S_t);
|
|
auto C_prime_t = std::get<0>(S_prime_t);
|
|
auto r_t = std::get<1>(S_t);
|
|
auto r_prime_t = std::get<1>(S_prime_t);
|
|
|
|
auto a = C_prime_t.x();
|
|
auto b = C_prime_t.y();
|
|
auto c = C_prime_t.z();
|
|
auto d = r_t * r_prime_t - C_t.x() * C_prime_t.x() - C_t.y() * C_prime_t.y() - C_t.z() * C_prime_t.z();
|
|
|
|
auto dist = std::abs(a * C_t.x() + b * C_t.y() + c * C_t.z() + d)/std::sqrt(a*a+b*b+c*c);
|
|
|
|
C_prime_t /= C_prime_t.norm();
|
|
auto C_P_t = C_t;
|
|
auto C_P_t1 = C_t + dist * C_prime_t;
|
|
auto C_P_t2 = C_t - dist * C_prime_t;
|
|
|
|
float dist1 = std::abs(a * C_P_t1.x() + b * C_P_t1.y() + c * C_P_t1.z() + d);
|
|
float dist2 = std::abs(a * C_P_t2.x() + b * C_P_t2.y() + c * C_P_t2.z() + d);
|
|
|
|
if (dist1 < dist2)
|
|
C_P_t = C_P_t1;
|
|
else
|
|
C_P_t = C_P_t2;
|
|
|
|
auto C_t_C_P_t = C_P_t - C_t;
|
|
|
|
auto r_P_t = r_t*r_t - C_t_C_P_t.norm2();
|
|
|
|
if (r_P_t < 0)
|
|
return {};
|
|
else
|
|
r_P_t = std::sqrt(r_P_t);
|
|
|
|
circlePoints.center = C_P_t;
|
|
v /= v.norm();
|
|
|
|
constexpr auto baseVector1 = Vector3<float>{1.0f,0.0f,0.0f};
|
|
auto baseVector2 = crossProduct(C_prime_t,baseVector1);
|
|
baseVector2 /= baseVector2.norm();
|
|
|
|
auto proj = (v*baseVector1)*baseVector1 + (v*baseVector2)*baseVector2;
|
|
proj /= proj.norm();
|
|
v = proj;
|
|
|
|
auto u = crossProduct(C_prime_t, v);
|
|
u /= u.norm();
|
|
|
|
for (unsigned int i = 0 ; i < nbPoints ; i++)
|
|
{
|
|
auto theta = 2 * M_PI * i /nbPoints ;
|
|
auto w = std::cos(theta) * v + std::sin(theta) * u;
|
|
auto circlePoint = C_P_t + r_P_t * w;
|
|
|
|
circlePoints.points.push_back(std::make_pair(i+offset,circlePoint));
|
|
}
|
|
|
|
return circlePoints;
|
|
}
|
|
|
|
template<typename... Types>
|
|
std::tuple<Types...> Spline<Types...>::operator()(float f) const
|
|
{
|
|
return detail::evalSpline(controlPoints, nodes, degree, f);
|
|
}
|
|
|
|
template<typename... Types>
|
|
std::tuple<Types...> Spline<Types...>::prime(float f) const
|
|
{
|
|
return detail::evalDerivativeSpline(controlPoints, nodes, degree, f);
|
|
}
|
|
|
|
template<typename... Types>
|
|
std::ostream& operator<<(std::ostream& out, Spline<Types...> const& spline)
|
|
{
|
|
out << "d " << spline.degree << "\n";
|
|
out << "n ";
|
|
|
|
for (auto const& node : spline.nodes)
|
|
out << node << " ";
|
|
out << "\n";
|
|
|
|
using namespace detail;
|
|
|
|
for (auto const& tuple : spline.controlPoints)
|
|
{
|
|
out << "p " << tuple << "\n";
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
template<typename... Types>
|
|
void Spline<Types...>::addControlPoint(std::string const& point)
|
|
{
|
|
std::tuple<Types...> tuple;
|
|
std::stringstream stream{point};
|
|
pae::detail::loadFromStream(tuple, stream);
|
|
controlPoints.push_back(tuple);
|
|
}
|
|
|
|
namespace detail
|
|
{
|
|
|
|
template<std::size_t I, std::size_t J, typename... Types>
|
|
struct evalSplineHelper
|
|
{
|
|
std::tuple<Types...> operator()(
|
|
std::vector<std::tuple<Types...>> const& controlPoints,
|
|
std::vector<float> const& nodes,
|
|
int degree,
|
|
float f,
|
|
std::tuple<Types...>& tuple)
|
|
{
|
|
std::get<J>(tuple) = 0;
|
|
|
|
for (auto i = 0u; i < nodes.size() - degree - 1; i++)
|
|
{
|
|
std::get<J>(tuple) += base(i, degree, f, nodes) * std::get<J>(controlPoints[i]);
|
|
}
|
|
|
|
evalSplineHelper<I-1,J+1,Types...>{}(controlPoints, nodes, degree, f, tuple);
|
|
|
|
return tuple;
|
|
}
|
|
};
|
|
|
|
template<std::size_t J, typename... Types>
|
|
struct evalSplineHelper<0,J,Types...>
|
|
{
|
|
std::tuple<Types...> operator()(
|
|
std::vector<std::tuple<Types...>> const& controlPoints,
|
|
std::vector<float> const& nodes,
|
|
int degree,
|
|
float f,
|
|
std::tuple<Types...>& tuple)
|
|
{
|
|
// Nothing to do here
|
|
return tuple;
|
|
}
|
|
|
|
};
|
|
|
|
template<typename... Types>
|
|
std::tuple<Types...> evalSpline(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f)
|
|
{
|
|
std::tuple<Types...> tuple;
|
|
|
|
return evalSplineHelper<std::tuple_size<std::tuple<Types...>>::value, 0, Types...>{}(
|
|
controlPoints, nodes, degree, f, tuple);
|
|
|
|
return tuple;
|
|
}
|
|
|
|
template<std::size_t I, std::size_t J, typename... Types>
|
|
struct evalDerivativeSplineHelper
|
|
{
|
|
|
|
std::tuple<Types...> operator()(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f, std::tuple<Types...>& tuple)
|
|
{
|
|
std::get<J>(tuple) = 0;
|
|
|
|
for (auto i = 0u; i < nodes.size() - degree -1 ; i++)
|
|
{
|
|
float denom1 = nodes[i+degree]-nodes[i];
|
|
float denom2 = nodes[i+degree+1]-nodes[i+1];
|
|
|
|
if (std::abs(denom1) > 0.001)
|
|
std::get<J>(tuple) += (degree/denom1)*base(i,degree-1,f,nodes)*std::get<J>(controlPoints[i]);
|
|
|
|
if (std::abs(denom2) > 0.001)
|
|
std::get<J>(tuple) -= (degree/denom2)*base(i+1,degree-1,f,nodes)*std::get<J>(controlPoints[i]);
|
|
}
|
|
|
|
// Appel recursif template
|
|
evalDerivativeSplineHelper<I-1,J+1,Types...>{}(controlPoints, nodes, degree, f, tuple);
|
|
|
|
return tuple;
|
|
}
|
|
};
|
|
|
|
template<std::size_t J,typename... Types>
|
|
struct evalDerivativeSplineHelper<0,J,Types...>
|
|
{
|
|
std::tuple<Types...> operator()(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f, std::tuple<Types...>& tuple)
|
|
{
|
|
// Nothing to do here
|
|
return tuple;
|
|
}
|
|
|
|
};
|
|
|
|
template<typename... Types>
|
|
std::tuple<Types...> evalDerivativeSpline(std::vector<std::tuple<Types...>> const& controlPoints, std::vector<float> const& nodes, int degree, float f)
|
|
{
|
|
std::tuple<Types...> tuple;
|
|
|
|
return evalDerivativeSplineHelper<std::tuple_size<std::tuple<Types...>>::value,0,Types...>{}(controlPoints, nodes, degree, f, tuple);
|
|
|
|
}
|
|
|
|
} // namespace detail
|
|
|
|
} // namespace geo
|