Commit 24725592 authored by student's avatar student
Browse files

update

parent 41dd9b18
......@@ -90,8 +90,8 @@ namespace PatternGeneratorJRL
friend std::ostream & operator<<(std::ostream &os, const struct COMState_s & acs);
};
typedef struct COMState_s COMState;
......@@ -99,7 +99,7 @@ namespace PatternGeneratorJRL
a sequence of relative positions. */
struct RelativeFootPosition_s
{
double sx,sy,theta;
double sx,sy,sz,theta;
double SStime;
double DStime;
int stepType; //1:normal walking 2:one step before obstacle
......
/*
* Copyright 2008, 2009, 2010,
* Copyright 2008, 2009, 2010,
*
* Francois Keith
* Olivier Stasse
......@@ -19,7 +19,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
* Research carried out within the scope of the
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
......@@ -34,16 +34,17 @@
using namespace PatternGeneratorJRL;
FootTrajectoryGenerationAbstract::FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
CjrlFoot *aFoot)
CjrlFoot *aFoot)
: SimplePlugin(lSPM)
{
m_Omega = 0.0;
m_Foot= aFoot;
m_Foot= aFoot;
m_SamplingPeriod = 0.005;
m_isStepStairOn =1;
std::string aMethodName[5] =
std::string aMethodName[5] =
{":omega",
":stepheight",
":stepheight",
":singlesupporttime",
":doublesupporttime",
":samplingperiod"};
......@@ -84,10 +85,10 @@ void FootTrajectoryGenerationAbstract::CallMethod(std::string &Method,
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & , //SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &, //NoneSupportFootAbsolutePositions,
int , //CurrentAbsoluteIndex,
int , //IndexInitial,
int , //CurrentAbsoluteIndex,
int , //IndexInitial,
double , //ModulatedSingleSupportTime,
int , //StepType,
int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-1: To be implemented ");
......@@ -95,11 +96,11 @@ void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolut
void FootTrajectoryGenerationAbstract::UpdateFootPosition(std::deque<FootAbsolutePosition> & ,//SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> & ,//NoneSupportFootAbsolutePositions,
int , // StartIndex,
int , // StartIndex,
int , //k,
double , //LocalInterpolationStartTime,
double , //ModulatedSingleSupportTime,
int , //StepType,
int , //StepType,
int ) //LeftOrRight)
{
LTHROW("FootTrajectoryGenerationAbstract::UpdateFootPosition-2: To be implemented ");
......
/*
* Copyright 2008, 2009, 2010,
* Copyright 2008, 2009, 2010,
*
* Olivier Stasse
*
......@@ -18,7 +18,7 @@
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
* Research carried out within the scope of the
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
......@@ -52,14 +52,14 @@ namespace PatternGeneratorJRL
/** @ingroup foottrajectorygeneration
This class defines the abstract interface to interact with foot generation object.
Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
Two parameters \f$ T_{DS} \f$ and \f$ T_{SS} \f$ defines respectively
the double support time and the single support time.
\f$ \omega \f$ defines the angle of the feet for landing and taking off.
The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
for taking off. Whereas for the landing the foot rotates around the
The foot rotates around the toe from \f$ 0 \f$ to \f$ \omega \f$
for taking off. Whereas for the landing the foot rotates around the
heel from \f$ \omega \f$ to \f$ 0 \f$.
The sampling control time indicates the amount of time between two
iteration of the algorithm. This parameter is used in the method
UpdateFootPosition to compute the time from the iteration number
......@@ -68,21 +68,21 @@ namespace PatternGeneratorJRL
An instance of a class derived from FootTrajectoryGenerationAbstract,
should call InitializeInternalDataStructures() once all the internal
parameters of the object are set.
The virtual function FreeInternalDataStructures() is used when changing some
parameters and by the destructor.
The most important function is UpdateFootPosition() which populates a
queue of foot absolute positions data structure.
queue of foot absolute positions data structure.
See a derived class such as FootTrajectoryGenerationStandard
for more precise information on the usage and sample codes.
See a derived class such as FootTrajectoryGenerationStandard
for more precise information on the usage and sample codes.
*/
class FootTrajectoryGenerationAbstract :public SimplePlugin
{
public:
/*! Constructor: In order to compute some appropriate strategies,
this class needs to extract specific details from the humanoid model. */
FootTrajectoryGenerationAbstract(SimplePluginManager *lSPM,
......@@ -99,7 +99,7 @@ namespace PatternGeneratorJRL
This position is supposed to be constant.
@param NoneSupportFootAbsolutePositions: Queue of absolute position for the swinging
foot. This method will set the foot position at index NoneSupportFootAbsolutePositions
of the queue.
of the queue.
@param CurrentAbsoluteIndex: Index in the queues of the foot position to be set.
@param IndexInitial: Index in the queues which correspond to the starting point
of the current single support phase.
......@@ -109,8 +109,8 @@ namespace PatternGeneratorJRL
*/
virtual void UpdateFootPosition(std::deque<FootAbsolutePosition> &SupportFootAbsolutePositions,
std::deque<FootAbsolutePosition> &NoneSupportFootAbsolutePositions,
int CurrentAbsoluteIndex,
int IndexInitial,
int CurrentAbsoluteIndex,
int IndexInitial,
double ModulatedSingleSupportTime,
int StepType, int LeftOrRight);
......@@ -123,56 +123,56 @@ namespace PatternGeneratorJRL
/*! Initialize internal data structures. */
virtual void InitializeInternalDataStructures()=0;
/*! Free internal data structures */
virtual void FreeInternalDataStructures()=0;
/*! \name Setter and getter for parameters
/*! \name Setter and getter for parameters
@{
*/
/*! \name Single Support Time
/*! \name Single Support Time
@{
*/
/*! \brief Set single support time */
void SetSingleSupportTime(double aTSingle)
{ m_TSingle =aTSingle;};
/*! \brief Get single support time */
double GetSingleSupportTime() const
{ return m_TSingle;};
/*! @} */
/*! \name Double Support Time
/*! \name Double Support Time
@{
*/
/*! \brief Set double support time */
void SetDoubleSupportTime(double aTDouble)
{ m_TDouble =aTDouble;};
/*! \brief Get single support time */
double GetDoubleSupportTime() const
{ return m_TDouble;};
/*! @}*/
/*! \name Sampling control Time
/*! \name Sampling control Time
@{
*/
/*! \brief Set single support time */
void SetSamplingPeriod(double aSamplingPeriod)
{ m_SamplingPeriod = aSamplingPeriod;};
/*! \brief Get single support time */
double GetSamplingPeriod() const
{ return m_SamplingPeriod;};
/*!@}*/
/*! \name Omega.
/*! \name Omega.
@{*/
/*! Get Omega */
......@@ -182,17 +182,17 @@ namespace PatternGeneratorJRL
/*! Set Omega */
void SetOmega(double anOmega)
{ m_Omega = anOmega; };
/*! @} */
/*! @} */
/*! \brief Reimplementation of the call method for the plugin manager.
/*! \brief Reimplementation of the call method for the plugin manager.
More explicitly this object will deal with the call which initialize
the feet behaviors (\f$omega\f$, \f$ stepheight \f$) .
*/
virtual void CallMethod(std::string &Method, std::istringstream &strm);
protected:
protected:
/*! Sampling period of the control. */
double m_SamplingPeriod;
......@@ -205,12 +205,14 @@ namespace PatternGeneratorJRL
/*! Store a pointer to Foot information. */
CjrlFoot * m_Foot;
/*! Omega the angle for taking off and landing. */
double m_Omega;
int m_isStepStairOn;
};
}
#endif /* _FOOT_TRAJECTORY_GENERATION_ABSTRACT_H_ */
......@@ -45,7 +45,7 @@ FootTrajectoryGenerationStandard::FootTrajectoryGenerationStandard(SimplePluginM
m_PolynomeOmega = 0;
m_PolynomeOmega2 = 0;
m_PolynomeTheta = 0;
m_isStepStairOn = 1;
// m_isStepStairOn = 1;
/* Computes information on foot dimension
from humanoid specific informations. */
......@@ -179,6 +179,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex,
else
{
m_BsplinesZ->SetParameters(TimeInterval,Position/1.5,TimeInterval/3.0,Position);
cout << "XXXXXXXXXXXXXXXXXXXXXXXXXXXX" << endl;
}
break;
......@@ -226,9 +227,8 @@ int FootTrajectoryGenerationStandard::SetParametersWithInitPosInitSpeed(int Poly
}
else
{
m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
m_BsplinesZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition,InitPosition,InitSpeed);
}
break;
case THETA_AXIS:
......@@ -271,7 +271,7 @@ int FootTrajectoryGenerationStandard::SetParameters(int PolynomeIndex, double Ti
}
else
{
m_BsplinesZ->SetParameters(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition);
m_BsplinesZ->SetParametersWithInitPosInitSpeed(TimeInterval,FinalPosition/1.5,TimeInterval/3.0,FinalPosition,InitPosition,InitSpeed);
}
break;
......@@ -444,7 +444,7 @@ double FootTrajectoryGenerationStandard::ComputeSecDerivative(unsigned int Polyn
}
else
{
r=m_BsplinesZ->ZComputeVelocity(Time);
r=m_BsplinesZ->ZComputeAcc(Time);
}
break;
......@@ -497,6 +497,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// Do not modify x, y and theta while liftoff.
curr_NSFAP.x = init_NSFAP.x;
curr_NSFAP.y = init_NSFAP.y;
//curr_NSFAP.z = init_NSFAP.z;
curr_NSFAP.theta = init_NSFAP.theta;
}
else if (LocalTime < StartLanding)
......@@ -504,6 +505,7 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// DO MODIFY x, y and theta the remaining time.
curr_NSFAP.x = init_NSFAP.x + m_PolynomeX->Compute(LocalTime - EndOfLiftOff);
curr_NSFAP.y = init_NSFAP.y + m_PolynomeY->Compute(LocalTime - EndOfLiftOff);
curr_NSFAP.theta = init_NSFAP.theta + m_PolynomeTheta->Compute(LocalTime - EndOfLiftOff);
}
else
......@@ -522,6 +524,8 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
{
curr_NSFAP.z = init_NSFAP.z + m_BsplinesZ->ZComputePosition(LocalTime);
}
ODEBUG2("x:" << curr_NSFAP.x << " LocalTime - EndOfLiftOff" << LocalTime - EndOfLiftOff
<< " " << m_PolynomeX->Compute(LocalTime - EndOfLiftOff));
// m_PolynomeX->print();
......@@ -693,6 +697,8 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dtheta =
m_PolynomeTheta->ComputeDerivative(LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff);
// +NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
else
{
......@@ -721,29 +727,37 @@ void FootTrajectoryGenerationStandard::UpdateFootPosition(deque<FootAbsolutePosi
// + NoneSupportFootAbsolutePositions[StartIndex].dtheta;
}
if (m_isStepStairOn == 1)
if (m_isStepStairOn == 1)
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
//m_AnklePositionRight[2];
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
//m_AnklePositionRight[2];
if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime)== 0.0)
{
cout << LocalInterpolationStartTime+InterpolationTime << " " << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].z << " " << NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z << endl;
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex-1].z +
m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
}
else{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z = //prev_NSFAP.z +
m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
}
}
else
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
//m_AnklePositionRight[2];
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//+
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);//
//m_AnklePositionRight[2];
}
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
......
......@@ -211,7 +211,7 @@ namespace PatternGeneratorJRL
/*! \brief Bsplines for Z axis position. */
ZBsplines *m_BsplinesZ;
int m_isStepStairOn;
// int m_isStepStairOn;
/*! \brief Foot dimension. */
double m_FootB, m_FootH, m_FootF;
......
......@@ -77,6 +77,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
// Do not modify x, y and theta while liftoff.
curr_NSFAP.x = prev_NSFAP.x;
curr_NSFAP.y = prev_NSFAP.y;
//curr_NSFAP.z = prev_NSFAP.z;
curr_NSFAP.theta = prev_NSFAP.theta;
}
else if (LocalInterpolationStartTime < EndOfLiftOff && LocalInterpolationStartTime +InterpolationTime > EndOfLiftOff)
......@@ -85,6 +86,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
double remainingTime = LocalInterpolationStartTime + InterpolationTime - EndOfLiftOff;
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(remainingTime);
// cout << "remain "<< prev_NSFAP.x << " " << remainingTime << " " << curr_NSFAP.x <<endl;
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(remainingTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(remainingTime);
......@@ -96,6 +98,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute(remainingTime);
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(remainingTime);
if(m_PolynomeTheta->Degree() > 4)
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(remainingTime);
}
......@@ -104,6 +107,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
// DO MODIFY x, y and theta all the time.
// x, dx
curr_NSFAP.x = m_PolynomeX->Compute(InterpolationTime);
// cout << "Inter "<< prev_NSFAP.x << " " << InterpolationTime << " " << curr_NSFAP.x <<endl;
curr_NSFAP.dx = m_PolynomeX->ComputeDerivative(InterpolationTime);
if(m_PolynomeX->Degree() > 4)
curr_NSFAP.ddx = m_PolynomeX->ComputeSecDerivative(InterpolationTime);
......@@ -112,6 +116,7 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.dy = m_PolynomeY->ComputeDerivative(InterpolationTime);
if(m_PolynomeY->Degree() > 4)
curr_NSFAP.ddy = m_PolynomeY->ComputeSecDerivative(InterpolationTime);
//theta, dtheta
curr_NSFAP.theta = m_PolynomeTheta->Compute( InterpolationTime );
curr_NSFAP.dtheta = m_PolynomeTheta->ComputeDerivative(InterpolationTime);
......@@ -119,14 +124,20 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
curr_NSFAP.ddtheta = m_PolynomeTheta->ComputeSecDerivative(InterpolationTime);
}
if (m_isStepStairOn == 1)
{
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].z =
m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
//m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
// m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
if (m_isStepStairOn == 1)
{
if (m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime) == 0.0)
{
//cout << LocalInterpolationStartTime+InterpolationTime << " " << prev_NSFAP.z << " " << curr_NSFAP.z << endl;
curr_NSFAP.z = prev_NSFAP.z;
curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
}
else{
curr_NSFAP.z = m_BsplinesZ->ZComputePosition(LocalInterpolationStartTime+InterpolationTime);
curr_NSFAP.dz = m_BsplinesZ->ZComputeVelocity(LocalInterpolationStartTime+InterpolationTime);
}
}
else{
......@@ -134,8 +145,8 @@ OnLineFootTrajectoryGeneration::UpdateFootPosition(deque<FootAbsolutePosition> &
m_PolynomeZ->Compute(LocalInterpolationStartTime+InterpolationTime);
NoneSupportFootAbsolutePositions[CurrentAbsoluteIndex].dz =
m_PolynomeZ->ComputeDerivative(LocalInterpolationStartTime+InterpolationTime);
}
bool ProtectionNeeded=false;
// Treat Omega with the following strategy:
......@@ -270,6 +281,7 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
FinalRightFootTraj_deq.resize((unsigned int)(QP_T_/m_SamplingPeriod)+CurrentIndex+1);
if(CurrentSupport.Phase == SS && Time+3.0/2.0*QP_T_ < CurrentSupport.TimeLimit)
{
// m_isStepStairOn = 1;
//determine coefficients of interpolation polynome
double ModulationSupportCoefficient = 0.9;
double UnlockedSwingPeriod = m_TSingle * ModulationSupportCoefficient;
......@@ -303,8 +315,14 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
);
if(CurrentSupport.StateChanged==true)
SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
{
if (m_isStepStairOn == 1)
SetParametersWithInitPosInitSpeed(FootTrajectoryGenerationStandard::Z_AXIS,TimeInterval,StepHeight_,LastSFP->z,LastSFP->dz);
else
SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
// SetParameters(FootTrajectoryGenerationStandard::Z_AXIS, m_TSingle, StepHeight_);
}
SetParametersWithInitPosInitSpeed(
FootTrajectoryGenerationStandard::THETA_AXIS,
TimeInterval, PreviewedSupportAngles_deq[0]*180.0/M_PI,
......@@ -344,6 +362,7 @@ OnLineFootTrajectoryGeneration::interpolate_feet_positions(double Time,
}
else if (CurrentSupport.Phase == DS || Time+3.0/2.0*QP_T_ > CurrentSupport.TimeLimit)
{
//m_isStepStairOn = 0;
for(int k = 0; k<=(int)(QP_T_/m_SamplingPeriod);k++)
{
FinalRightFootTraj_deq[CurrentIndex+k]= FinalRightFootTraj_deq[CurrentIndex+k-1];
......
......@@ -97,7 +97,6 @@ void Bsplines::GenerateKnotVector(std::string method)
cout << "Knot vector cant be created. m_control_points.size()-1>=m_degree "<< endl;
m_knot_vector.clear();
}
}
else if (method =="universal")
......@@ -296,11 +295,11 @@ void Bsplines::PrintDegree() const
}
// Class ZBplines heritage of class Bsplines
// create a foot trajectory of Z on function the time t
/// Class ZBplines heritage of class Bsplines
/// create a foot trajectory of Z on function the time t
ZBsplines::ZBsplines(double FT, double FP, double ToMP, double MP):Bsplines(4)
ZBsplines::ZBsplines( double FT, double FP, double ToMP, double MP):Bsplines(4)
{
SetParameters(FT, FP, ToMP, MP);
}
......@@ -351,16 +350,25 @@ double ZBsplines::ZComputeAcc(double t)
void ZBsplines::SetParameters(double FT, double FP, double ToMP, double MP)
{
ZGenerateKnotVector(FT,ToMP);
ZGenerateControlPoints(FT, FP, ToMP, MP);
ZGenerateControlPoints(0.0,FT, FP, ToMP, MP);
}
void ZBsplines::SetParametersWithInitPosInitSpeed(double FT, double FP, double ToMP, double MP,
double InitPos,
double InitSpeed)
{
ZGenerateKnotVector(FT,ToMP);
ZGenerateControlPoints(InitPos,FT, FP, ToMP, MP);
}
void ZBsplines::GetParametersWithInitPosInitSpeed(double &FT,
double &FP,
double &MP,
double &InitPos,
double &InitSpeed)
{
FT = m_FT;
FP = m_FP;
MP = m_MP;
InitPos = ZComputePosition(0.0);
InitSpeed = ZComputeVelocity(0.0);
}
......@@ -382,50 +390,50 @@ void ZBsplines::ZGenerateKnotVector(double FT, double ToMP)
{
knot.push_back(FT);
}
SetKnotVector(knot);
}
void ZBsplines::ZGenerateControlPoints(double FT, double FP, double ToMP, double MP)
void ZBsplines::ZGenerateControlPoints(double IP,double FT, double FP, double ToMP, double MP)
{
m_FT = FT;
m_FP = FP;
m_ToMP = ToMP;
m_MP = MP;
std::vector<Point> control_points;
control_points.clear();
std::ofstream myfile1;
myfile1.open("control_point.txt");
Point A = {0.0,0.0};
Point A = {0.0,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
A = {m_FT*0.05,0.0};
A = {m_FT*0.05,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
A = {m_FT*0.1,0.0};
A = {m_FT*0.1,IP};
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
A = {0.85*m_ToMP,m_MP};
A = {0.85*m_ToMP,IP+