-
Notifications
You must be signed in to change notification settings - Fork 12
/
HSplineCore.h
166 lines (145 loc) · 4.25 KB
/
HSplineCore.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
#pragma once
#include "HSSSplineSample.h"
#include <LinearSystemLib.h>
#include <vector>
namespace HSSSpline
{
template <int D>
class HSplineCore
{
protected:
//曲線參數
struct Spline_Ele
{
double val[D][4];
};
std::vector< Spline_Ele > m_LineSeg_List;
//建Spline
public:
virtual int BuildingSpline(PathPoints<D> &_points)
{
if (_points().size() < 2)
{
std::cerr << "Cubic spline point number less than 2!\n";
return 1;
}
using namespace LinearSystemLib;
/*建立A矩陣*/
GeneralSparseMatrix* GA = new GeneralSparseMatrix;
GA->Create( _points().size(), _points().size());
setup_matrixA(*GA, _points().size());
//建立B矩陣
// only support double type.
//-------------------------------------------------
const int dim = D;//x, y, w, h1, h2各一組
double** B = new double*[dim]; // create 2d.
for (int i=0;i<dim;i++){B[i] = new double[_points().size()];}
for (unsigned int i = 0; i < _points().size(); ++i)
{
if (i == 0)
{
for (int d=0;d<dim;d++){B[d][i] = 3*(_points[1][d] - _points[0][d]);}
}else if (i == _points().size()-1)
{
for (int d=0;d<dim;d++){B[d][i] = 3*(_points[i][d] - _points[i-1][d]);}
}else{
for (int d=0;d<dim;d++){B[d][i] = 3*(_points[i+1][d] - _points[i-1][d]);}
}
}
// create the linear system A x = B.
//-------------------------------------------------
// note: 建立的 A and B memory 都將由 sls 負責管理,
// ps. A and B must allocate from heap.
// ps. GA 須轉成, stable sparse matrix.
SparseLinearSystem *sls = new SparseLinearSystem( new StableSparseMatrix(*GA), B, dim );
// solving it !! A * x = B.
//-------------------------------------------------
try
{
// the solution will be stored here.
double** S = 0;
// solve it !!
bool result = GeneralSparseLSSolver::GetInstance()->Solve( sls, S );
// output the solution..!!
//-------------------------------------------------
if( !result )
cerr << "solve error!!! check your matrix." << endl;
else
{
m_LineSeg_List.clear();
for (unsigned int i = 0; i < _points().size()-1; ++i)
{
Spline_Ele ele;
for (int d=0;d<dim;d++)
{
ele.val[d][0] = (double)( S[d][i+1] + S[d][i] - 2 * (_points[i+1][d] - _points[i][d]) );
ele.val[d][1] = (double)( 3 * (_points[i+1][d] - _points[i][d]) - 2 * S[d][i] - S[d][i+1] );
ele.val[d][2] = (double)( S[d][i] );
ele.val[d][3] = (double)( _points[i][d] );
}
m_LineSeg_List.push_back(ele);
}
}
// 須要自行 release solution memory, A,B 不用!!
//-------------------------------------------------
if (S!=0)
{
for( int i = 0 ; i < dim ; ++i )delete[] S[i];
delete[] S;
S = NULL;
}
}
catch( exception e )
{
cerr << e.what() << endl;
}
delete sls;
return 0;
}
private:
void setup_matrixA(LinearSystemLib::GeneralSparseMatrix& _matrix, int _size)
{
_matrix.SetElement( 0, 0, 2.0);
for (int i = 1; i < _size-1; ++i)
{
_matrix.SetElement(i, i, 4.0);
}
for (int i = 0; i < _size-1; ++i)
{
_matrix.SetElement(i, i+1, 1.0);
_matrix.SetElement(i+1, i, 1.0);
}
_matrix.SetElement( _size-1, _size-1, 2.0 );
}
//Get Info
public:
/*取得段數*/
int n_segs(){return m_LineSeg_List.size();}
double get_value(int dim,int _seg, double _t)
{
double t2 = _t*_t, t3 = t2*_t;
return (double)( m_LineSeg_List[_seg].val[dim][0] * t3 + m_LineSeg_List[_seg].val[dim][1] * t2 + m_LineSeg_List[_seg].val[dim][2] * _t + m_LineSeg_List[_seg].val[dim][3] );
}
//一次微分
double get_D1_value(int dim,int _seg, double _t)
{
double t2 = _t*_t;
return (double)( 3 * m_LineSeg_List[_seg].val[dim][0] * t2 + 2 * m_LineSeg_List[_seg].val[dim][1] * _t + m_LineSeg_List[_seg].val[dim][2] );
}
double get_D2_value(int dim,int _seg, double _t)
{
return (double)( 6 * m_LineSeg_List[_seg].val[dim][0] * _t + 2 * m_LineSeg_List[_seg].val[dim][1]);
}
/*取得spline上的某一點, seg是哪一段, t是參數*/
PathPoint<D> get_point(int _seg, double _t)
{
HSSSpline::PathPoint<D> p;
for (int i=0;i<D;i++)
{
p[i] = get_value(i,_seg,_t);
}
return p;
}
PathPoint<D> get_point(const Sample& sample){return get_point(sample.seg_idx,sample._t);}
};
}