00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <common/SplineCurve.h>
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 static fixed splineBlend(int k,int t,int *u,fixed v)
00036 {
00037 fixed value;
00038
00039 if (t == 1) {
00040 if ((v > u[k]) && (v < u[k+1]))
00041 value = 1;
00042 else
00043 value = 0;
00044 } else {
00045 if ((u[k+t-1] == u[k]) && (u[k+t] == u[k+1]))
00046 value = 0;
00047 else if (u[k+t-1] == u[k])
00048 value = (fixed(u[k+t]) - v) / (u[k+t] - u[k+1]) * splineBlend(k+1,t-1,u,v);
00049 else if (u[k+t] == u[k+1])
00050 value = (v - u[k]) / (u[k+t-1] - u[k]) * splineBlend(k,t-1,u,v);
00051 else
00052 value = (v - u[k]) / (u[k+t-1] - u[k]) * splineBlend(k,t-1,u,v) +
00053 (fixed(u[k+t]) - v) / (u[k+t] - u[k+1]) * splineBlend(k+1,t-1,u,v);
00054 }
00055 return(value);
00056 }
00057
00058
00059
00060
00061
00062
00063 static void splinePoint(int *u,int n,int t,fixed v,
00064 std::vector<FixedVector> &control, FixedVector &output)
00065 {
00066 output[0] = 0;
00067 output[1] = 0;
00068 output[2] = 0;
00069
00070 for (int k=0;k<=n;k++) {
00071 fixed b = splineBlend(k,t,u,v);
00072 output[0] += (control[k])[0] * b;
00073 output[1] += (control[k])[1] * b;
00074 output[2] += (control[k])[2] * b;
00075 }
00076 }
00077
00078
00079
00080
00081
00082
00083
00084
00085 static void splineKnots(int *u,int n,int t)
00086 {
00087 int j;
00088
00089 for (j=0;j<=n+t;j++) {
00090 if (j < t)
00091 u[j] = 0;
00092 else if (j <= n)
00093 u[j] = j - t + 1;
00094 else if (j > n)
00095 u[j] = n - t + 2;
00096 }
00097 }
00098
00099
00100
00101
00102
00103
00104
00105 static void splineCurve(std::vector<FixedVector> &inp_list,
00106 int n,int *knots,int t,
00107 std::vector<FixedVector> &outp_list,int res)
00108 {
00109 int i;
00110 fixed interval,increment;
00111
00112 interval = fixed(true, 10);
00113 increment = fixed(n - t + 2) / fixed(res - 1);
00114 for (i=0;i<=res-1;i++) {
00115
00116 FixedVector outp;
00117 splinePoint(knots,n,t,interval,inp_list,outp);
00118 outp_list.push_back(outp);
00119 interval += increment;
00120 }
00121 outp_list[res-1] = inp_list[n];
00122 }
00123
00124 void SplineCurve::generate(
00125 std::vector<FixedVector> &inPoints,
00126 std::vector<FixedVector> &outPoints,
00127 int resolution,
00128 int polynomials)
00129 {
00130 int N = int(inPoints.size()) - 1;
00131 int T = polynomials;
00132 int *knots = new int[N + T + 1];
00133
00134 splineKnots(knots,N,T);
00135 splineCurve(inPoints,N,knots,T,outPoints,resolution);
00136
00137 delete [] knots;
00138 }