void spline::renderAsRollerCoaster(int resolution, float width){
cubic_spline(resolution,0);
glPushMatrix();
glTranslatef(0,0,width);
cubic_spline(resolution,0);
glPopMatrix();
}
/**
* The function creates the quadratic spline curve
*
* @param c_points control points
* @param smoothness smoothness to achieve
* @param curve holds the smooth curve
*/
void CubicSpline::cubic_spline( const std::vector<Point>& c_points,
const float& smoothness,
std::vector<Point>* curve) const {
if(is_smooth(c_points, smoothness)) {
for(unsigned int i = 0, i_end = c_points.size(); i < i_end; ++i) {
curve->push_back(c_points.at(i));
}
} else {
std::vector<Point>* sub_points = new std::vector<Point>();
sub_points->push_back(*c_points.begin());
for(int i = 0, i_end = c_points.size() -1; i < i_end; ++i) {
sub_points->push_back(c_points.at(i) * 1/2 + c_points.at(i+1) * 1/2);
if(i < i_end - 1) {
sub_points->push_back(c_points.at(i) * 1/8
+ c_points.at(i+1) * 6/8
+ c_points.at(i+2) * 1/8);
}
}
sub_points->push_back(*(c_points.end()-1));
cubic_spline(*sub_points, smoothness, curve);
delete sub_points;
}
}
/********************************************************************************
void compute_grid_bound(int nb, const couble *bnds, const int *npts, int *grid_size, const char *center_cell)
compute the 1-D grid location.
********************************************************************************/
double* compute_grid_bound(int nb, const double *bnds, const int *npts, int *grid_size, const char *center)
{
int refine, i, n, np;
double *grid=NULL, *tmp=NULL;
double *grid1=NULL, *grid2=NULL;
if(!strcmp(center, "none") )
refine = 1;
else if(!strcmp(center, "t_cell") || !strcmp(center, "c_cell") )
refine = 2;
else
mpp_error("tool_util: center should be 'none', 'c_cell' or 't_cell' ");
grid1 = (double *)malloc(nb*sizeof(double));
grid1[0] = 1;
n = 0;
for(i=1; i<nb; i++) {
if(npts[i-1]%refine) mpp_error("tool_util: when center_cell is not 'none', npts should be divided by 2");
n += npts[i-1]/refine;
grid1[i] = n+1;
}
np = n + 1;
*grid_size = n*refine;
tmp = (double *)malloc(np*sizeof(double));
grid = (double *)malloc((*grid_size+1)*sizeof(double));
grid2 = (double *)malloc(np*sizeof(double));
for(i=0;i<np;i++) grid2[i] = i + 1.0;
cubic_spline( nb, np, grid1, grid2, bnds, tmp, 1e30, 1e30);
if(!strcmp(center, "none")) {
for(i=0; i<np; i++) grid[i] = tmp[i];
}
else if(!strcmp(center, "t_cell")) {
for(i=0; i<np; i++) grid[2*i] = tmp[i];
for(i=0; i<n; i++) grid[2*i+1] = 0.5*(tmp[i]+tmp[i+1]);
}
else if( !strcmp(center, "c_cell")) {
for(i=0; i<np; i++) grid[2*i] = tmp[i];
grid[1] = 0.5*(tmp[0]+tmp[1]);
for(i=1; i<n; i++) grid[2*i+1] = 2*grid[2*i] - grid[2*i-1];
}
free(grid1);
free(grid2);
free(tmp);
return grid;
};/* compute_grid_bound */
/**
* Method to check if the point specified, is on the object / on a point of the
* curve
*
* @param x x position of the point to be checked
* @param y y position of the point to be checked
* @param selected true if the object is selected
* @return point 'under' x and y or NULL
*/
Point* CubicSpline::on_object(const unsigned int& x,
const unsigned int& y,
const bool& selected) const {
if(selected) { // if selected only check if on control polygon
return Polygon::on_object(x, y, selected);
} else {
std::vector<Point>* curve = new std::vector<Point>();
cubic_spline(*object_, LOW_SMOOTHNESS, curve); // calculate the curve with
// low polygon count
Point* ret = on_edge(x, y, *curve);
delete curve;
return ret;
}
return NULL;
}
/**
* draw internal is used only by the component objects them self.
* The method draws the curve
*/
void CubicSpline::draw_internal() const {
std::vector<Point>* curve = new std::vector<Point>();
cubic_spline(*object_, HIGH_SMOOTHNESS, curve);
// std::cout << "Points: " << curve->size() << std::endl;
glBegin(GL_LINE_STRIP);
for(std::vector<Point>::iterator i = curve->begin(); i != curve->end(); ++i) {
glVertex2f(i->x, i->y);
}
glEnd();
delete curve;
}
bool HuboPath::Trajectory::_spline_interpolation(const Eigen::VectorXd& velocities,
const Eigen::VectorXd& accelerations,
double frequency)
{
IndexArray joint_mapping;
get_active_indices(joint_mapping);
std::vector<Eigen::VectorXd> waypoints;
Eigen::VectorXd next_point(joint_mapping.size());
for(size_t i=0; i<elements.size(); ++i)
{
for(size_t j=0; j<joint_mapping.size(); ++j)
{
next_point[j] = elements[i].references[joint_mapping[j]];
}
waypoints.push_back(next_point);
}
spline_interpolation::Spline cubic_spline(waypoints, velocities, accelerations, frequency);
if(!cubic_spline.valid())
{
std::cout << "ERROR: Could not produce a valid cubic spline interpolation!" << std::endl;
return false;
}
std::vector<Eigen::VectorXd> interpolation = cubic_spline.getTrajectory();
elements.clear();
elements.resize(interpolation.size());
for(size_t i=0; i<interpolation.size(); ++i)
{
Eigen::VectorXd& point = interpolation[i];
for(size_t j=0; j<joint_mapping.size(); ++j)
{
elements[i].references[joint_mapping[j]] = point[j];
}
}
params.interp = HUBO_PATH_RAW;
std::cout << "Successfully performed a spline interpolation" << std::endl;
return true;
}
DBL Attenuate_Light (const LightSource *Light, const Ray &ray, DBL Distance)
{
DBL len, k, costheta;
DBL Attenuation = 1.0;
Vector3d P, V1;
/* If this is a spotlight then attenuate based on the incidence angle. */
switch (Light->Light_Type)
{
case SPOT_SOURCE:
costheta = dot(ray.Direction, Light->Direction);
if(Distance>0.0) costheta = -costheta;
if (costheta > 0.0)
{
Attenuation = pow(costheta, Light->Coeff);
if (Light->Radius > 0.0 && costheta < Light->Radius)
{
Attenuation *= cubic_spline(Light->Falloff, Light->Radius, costheta);
}
}
else
{
return 0.0; //Attenuation = 0.0;
}
break;
case CYLINDER_SOURCE:
// Project light->point onto light direction
// to make sure that we're on the correct side of the light
V1 = ray.Origin - Light->Center;
k = dot(V1, Light->Direction);
if (k > 0.0)
{
// Now subtract that from the light-direction. This will
// give us a vector showing us the distance from the
// point to the center of the cylinder.
P = V1 - k * Light->Direction;
len = P.length();
if (len < Light->Falloff)
{
DBL dist = 1.0 - len / Light->Falloff;
Attenuation = pow(dist, Light->Coeff);
if (Light->Radius > 0.0 && len > Light->Radius)
{
Attenuation *= cubic_spline(0.0, 1.0 - Light->Radius / Light->Falloff, dist);
}
}
else
{
return 0.0; //Attenuation = 0.0;
}
}
else
{
return 0.0; //Attenuation = 0.0;
}
break;
}
if (Attenuation > 0.0)
{
/* Attenuate light due to light source distance. */
if (Light->Fade_Power > 0.0)
{
if (fabs(Light->Fade_Distance) >= EPSILON)
Attenuation *= 2.0 / (1.0 + pow(Distance / Light->Fade_Distance, Light->Fade_Power));
else
Attenuation *= pow(Distance, -Light->Fade_Power);
}
}
return(Attenuation);
}