3dpcp/.svn/pristine/29/29c6895e972d84b5309d12ab77c2a0c561b90df1.svn-base
2012-09-16 14:33:11 +02:00

194 lines
5.1 KiB
Text

/*
* NurbsPath implementation
*
* Copyright (C) Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz,
Andreas Nuechter, Kai Lingemann, Jan Elseberg
*
* Released under the GPL version 3.
*
*/
#include "show/NurbsPath.h"
#include <stdio.h>
#include <iostream>
/**
* Smoothes a given path, using the Cox-De-Boor algorithm to compute
* new nodes on the B-Spline.
* @param origP vector of PointXYs representing the path
* @param nump number of points, which should be computed for the smoothed path
* @return vector of PointXYs representing the smoothed path
*/
vector<double> NurbsPath::camRatio = vector<double>();
vector<PointXY>
NurbsPath::getNurbsPath(vector<PointXY>& origP,
unsigned int nump, int inter_by_dist){
list<PGNode*> pgnl;
list<PGNode*> freepgnl;
for(unsigned int i=0; i<origP.size();i++){
PGNode* p = new PGNode();
p->point.x = origP[i].x;
p->point.y = origP[i].y;
pgnl.push_back(p);
freepgnl.push_back(p);
}
vector<PointXY> vec = getNurbsPath(pgnl,nump,inter_by_dist);
for(list<PGNode*>::iterator it = freepgnl.begin();it!=freepgnl.end();it++) {
PGNode *p = *it;
delete p;
}
pgnl.clear();
freepgnl.clear();
return vec;
}
/**
* Smoothes a given path, using the Cox-De-Boor algorithm to compute
* new nodes on the B-Spline.
* @param origP list of PGNode*s representing the path
* @param nump number of points, which should be computed for the smoothed path
* @return vector of PointXYs representing the smoothed path
*/
vector<PointXY>
NurbsPath::getNurbsPath(list<PGNode*>& origP,
unsigned int nump, int inter_by_dist){
ivN = origP.size();
if(ivN<=1){
vector<PointXY> pv;
return pv;
}
//hack for short paths
if(ivN==2){
list<PGNode*>::iterator it = origP.begin();
PGNode* tmpStart = *it;
it++;
origP.push_back(*it);
origP.push_front(tmpStart);
ivN=4;
}
if(ivN==3){
list<PGNode*>::iterator it = origP.begin();
it++;
origP.insert(it, (*it));
ivN=4;
}
//end short path hack
float *origPX = new float[ivN];
float *origPY = new float[ivN];
list<PGNode*>::iterator it;
unsigned int i=0;
for(it=origP.begin();it!=origP.end();it++,i++){
origPX[i] = (*it)->point.x;
origPY[i] = (*it)->point.y;
}
ivDegree = 3;
ivOrder = ivDegree+1;
ivNumKnots = ivN + ivDegree;
if(inter_by_dist) {
ivNumP = nump;
} else {
ivNumP = camRatio.size();
}
float *ivpKnots = new float[ivNumKnots+1];
for(i=0;i<=ivDegree;i++)
ivpKnots[i] = 0.0f;
int numIntKnots = ((ivNumKnots-ivDegree)-(ivOrder));
for(i=ivOrder; i < ivNumKnots - ivDegree ; i++)
ivpKnots[i] = ((float)(i-ivDegree))/(numIntKnots+1);
for(i=ivNumKnots-ivDegree;i<=ivNumKnots;i++)
ivpKnots[i] = 1.f;
vector<PointXY> nurbsPath;
nurbsPath.reserve(ivNumP);
PointXY outPoint;
float out[2]={0,0};
for(i=0;i<=ivNumP; i++){
out[0] = 0;
out[1] = 0;
if(i!=ivNumP)
if(inter_by_dist) {
getOutpoint((float)i/ivNumP,out,origPX,origPY,ivpKnots);
} else {
getOutpoint(camRatio[i],out,origPX,origPY,ivpKnots);
}
else
getOutpoint(.999999,out,origPX,origPY,ivpKnots);
outPoint.x = out[0];
outPoint.y = out[1];
nurbsPath.push_back(outPoint);
}
delete [] origPX;
delete [] origPY;
delete [] ivpKnots;
return nurbsPath;
}
/**
* Implementation of the Cox-De-Boor algorithm.
* The implementation of this algorithm is inspired by the Nurbs Curve Example on
* http://www.robthebloke.org/opengl_programming.html
* @param u measure for the length already gone on the smoothed path for the point to be calculated (it should hold that 0 <= u < 1)
* @param i effect of which original path node on the output path should be computed
* @param k order (degree of the B-Splines plus 1)
* @param Knots array of knots needed to compute the B-Spline
*/
float
NurbsPath::coxDeBoor(float u,int i,int k,const float* Knots) {
if(k==1)
{
if( Knots[i] <= u && u < Knots[i+1] ) {
return 1.0f;
}
return 0.0f;
}
float Den1 = Knots[i+k-1] - Knots[i];
float Den2 = Knots[i+k] - Knots[i+1];
float Eq1=0,Eq2=0;
if(Den1>0) {
Eq1 = ((u-Knots[i]) / Den1) * coxDeBoor(u,i,k-1,Knots);
}
if(Den2>0) {
Eq2 = (Knots[i+k]-u) / Den2 * coxDeBoor(u,i+1,k-1,Knots);
}
return Eq1+Eq2;
}
/**
* Computes one value on the B-Spline
* @param[in] t which point on the spline you want to compute (betwenn 0 for the start point and 1-epsilon for the end point of the smoothed path)
* @param[out] OutPoint[] array of two floats, will be overwritten with the x and y coordinate of the point
*/
void
NurbsPath::getOutpoint(float t,float OutPoint[],float*opx,float*opy,const float*knots) {
// sum the effect of all CV's on the curve at this point to
// get the evaluated curve point
//
for(unsigned int i=0;i!=ivN;++i) {
// calculate the effect of this point on the curve
float Val = coxDeBoor(t,i,ivOrder,knots);
if(Val>0.0001f) {
// sum effect of CV on this part of the curve
OutPoint[0] += Val * opx[i];
OutPoint[1] += Val * opy[i];
}
}
}