29 float *f1,
float *f2,
float *f1d,
float *f2d,
float *f1dd,
float *f2dd,
41 float nrf1 = cosf(wb*w1)*(r*cosf(wb*w2) + ratio);
42 float nrf2 = sinf(wb*w1)*(r*cosf(wb*w2) + ratio);
44 float nrf1d = -w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - cosf(wb*w1)*r*w2*sinf(wb*w2);
45 float nrf2d = w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) - sinf(wb*w1)*r*w2*sinf(wb*w2);
47 float nrf1dd = -w1*w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) - cosf(wb*w1)*r*w2*w2*cosf(wb*w2);
48 float nrf2dd = -w1*w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - sinf(wb*w1)*r*w2*w2*cosf(wb*w2);
50 *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
51 *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
53 *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
54 *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
56 *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
57 *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
61 float *f1,
float *f2,
float *f3,
float *f1d,
float *f2d,
float *f3d,
float *f1dd,
float *f2dd,
float *f3dd,
72 *f1 = r * cosf(wb) + xo;
73 *f2 = r * sinf(wb) + yo;
74 *f3 = 0.5 * (zh + zl + (zl - zh) * sinf(alpha_rad - wb));
78 *f3d = -0.5 * (zl - zh) * cosf(alpha_rad - wb);
80 *f1dd = -r * cosf(wb);
81 *f2dd = -r * sinf(wb);
82 *f3dd = -0.5 * (zl - zh) * sinf(alpha_rad - wb);
86 float *f1,
float *f2,
float *f3,
float *f1d,
float *f2d,
float *f3d,
float *f1dd,
float *f2dd,
float *f3dd,
105 float nrf1 = cx*cosf(wx*wb + deltax_rad);
106 float nrf2 = cy*cosf(wy*wb + deltay_rad);
108 *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
109 *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
110 *f3 = cz*cosf(wz*wb + deltaz_rad) + zo;
112 float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad);
113 float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad);
115 *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
116 *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
117 *f3d = -wz*cz*sinf(wz*wb + deltaz_rad);
119 float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad);
120 float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad);
122 *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
123 *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
124 *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad);
130 bezier_t *bezier,
float *f1,
float *f2,
float *f1d,
float *f2d,
float *f1dd,
float *f2dd,
135 int n_seg = floorl(t);
136 float tt = t - n_seg;
142 float p0x = bezier[n_seg].
p0[0];
float p0y = bezier[n_seg].
p0[1];
143 float p1x = bezier[n_seg].
p1[0];
float p1y = bezier[n_seg].
p1[1];
144 float p2x = bezier[n_seg].
p2[0];
float p2y = bezier[n_seg].
p2[1];
145 float p3x = bezier[n_seg].
p3[0];
float p3y = bezier[n_seg].
p3[1];
150 *f1 = (1 - tt) * (1 - tt) * (1 - tt) * p0x + 3 * (1 - tt) * (1 - tt) * tt * p1x + 3 *
151 (1 - tt) * tt * tt * p2x + tt * tt * tt * p3x;
152 *f2 = (1 - tt) * (1 - tt) * (1 - tt) * p0y + 3 * (1 - tt) * (1 - tt) * tt * p1y + 3 *
153 (1 - tt) * tt * tt * p2y + tt * tt * tt * p3y;
156 *f1d = 3 * (1 - tt) * (1 - tt) * (p1x - p0x) + 6 * (1 - tt) * tt * (p2x - p1x) + 3 * tt * tt * (p3x - p2x);
157 *f2d = 3 * (1 - tt) * (1 - tt) * (p1y - p0y) + 6 * (1 - tt) * tt * (p2y - p1y) + 3 * tt * tt * (p3y - p2y);
160 *f1dd = 6 * (1 - tt) * (p2x - 2 * p1x + p0x) + 6 * tt * (p3x - 2 * p2x + p1x);
161 *f2dd = 6 * (1 - tt) * (p2y - 2 * p1y + p0y) + 6 * tt * (p3y - 2 * p2y + p1y);
void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd, float wb)
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd, float wb)
void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd, float wb)
gvf_parametric_tra gvf_parametric_trajectory
void gvf_parametric_2d_bezier_splines_info(bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd, float w)