SLProject  4.2.000
A platform independent 3D computer graphics framework for desktop OS, Android, iOS and online in web browsers
SLCurveBezier.cpp
Go to the documentation of this file.
1 /**
2  * \file math/SLCurveBezier.cpp
3  * \date July 2014
4  * \authors Marcus Hudritsch
5  * \copyright http://opensource.org/licenses/GPL-3.0
6  * \remarks Please use clangformat to format the code. See more code style on
7  * https://github.com/cpvrlab/SLProject4/wiki/SLProject-Coding-Style
8 */
9 
10 #include <math/SLCurveBezier.h>
11 #include <SLGLState.h>
12 #include <SLScene.h>
13 
14 //-------------------------------------------------------------------------------
16 {
17  _totalLength = 0.0f;
18  SLVVec3f ctrls;
19  init(points, ctrls);
20 }
21 //-------------------------------------------------------------------------------
23  const SLVVec3f& controlPoints)
24 {
25  _totalLength = 0.0f;
26 
27  init(points, controlPoints);
28 }
29 //-------------------------------------------------------------------------------
31 {
32  dispose();
33 }
34 //-------------------------------------------------------------------------------
35 /*!
36 Init curve with curve points and times. If no control
37 points are passed they will be calculated automatically
38 @param points Array of points on the Bezier curve (w = time)
39 @param controlPoints Array of control points with size = 2*(numPointsAndTimes-1)
40 */
41 void SLCurveBezier::init(const SLVVec4f& points,
42  const SLVVec3f& controlPoints)
43 {
44  assert(points.size() > 1);
45 
46  dispose();
47 
48  // set up arrays
49  _points.clear();
50  _points.resize(points.size());
51  _controls.resize(2 * (points.size() - 1));
52 
53  // copy interpolated data
54  SLuint i;
55  for (i = 0; i < _points.size(); ++i)
56  {
57  _points[i] = points[i];
58  }
59 
60  if (controlPoints.empty())
61  {
62  if (points.size() > 2)
63  {
64  // create approximating control points
65  for (i = 0; i < _points.size() - 1; ++i)
66  {
67  if (i > 0)
68  _controls[2 * i] = (_points[i] + (_points[i + 1] - _points[i - 1]) / 3.0f).vec3();
69  if (i < _points.size() - 2)
70  _controls[2 * i + 1] = (_points[i + 1] - (_points[i + 2] - _points[i]) / 3.0f).vec3();
71  }
72 
73  _controls[0] = (SLVec4f(_controls[1]) - (_points[1] - _points[0]) / 3.0f).vec3();
74  _controls[2 * _points.size() - 3] = (SLVec4f(_controls[2 * _points.size() - 4]) +
75  (_points[_points.size() - 1] - _points[_points.size() - 2]) / 3.0f)
76  .vec3();
77  }
78  else
79  {
80  _controls[0] = (_points[0] + (_points[1] - _points[0]) / 3.0f).vec3();
81  _controls[1] = (_points[1] - (_points[1] - _points[0]) / 3.0f).vec3();
82  }
83  }
84  else
85  { // copy approximating control points
86  for (i = 0; i < 2 * (_points.size() - 1); ++i)
87  _controls[i] = controlPoints[i];
88  }
89 
90  // set up curve segment lengths
91  _lengths.clear();
92  _lengths.resize(_points.size() - 1);
93  _totalLength = 0.0f;
94 
95  _totalLength = 0.0f;
96  for (SLuint i = 0; i < _points.size() - 1; ++i)
97  {
98  _lengths[i] = segmentArcLength(i, 0.0f, 1.0f);
99  _totalLength += _lengths[i];
100  }
101 }
102 //-----------------------------------------------------------------------------
103 /*!
104 SLCurveBezier::draw does the rendering of the Bezier curve in world space.
105 */
107 {
108  SLint numControlPoints = 2 * ((SLint)_points.size() - 1);
109 
110  // Create buffer object
111  if (!_vao.vaoID())
112  {
113  // Build renderPoints by recursively subdividing the curve
114  SLVVec3f renderPoints;
115  for (SLuint i = 0; i < _points.size() - 1; ++i)
116  {
117  subdivideRender(renderPoints,
118  wm,
119  0.00001f,
120  _points[i].vec3(),
121  _controls[2 * i],
122  _controls[2 * i + 1],
123  _points[i + 1].vec3());
124  }
125 
126  // add last point to the curve vector
127  renderPoints.push_back(wm.multVec(_points.back().vec3()));
128 
129  // add inputs points
130  for (SLuint i = 0; i < _points.size(); ++i)
131  renderPoints.push_back(wm.multVec(_points[i].vec3()));
132 
133  // add control points
134  for (SLuint i = 0; i < (SLuint)numControlPoints; ++i)
135  renderPoints.push_back(wm.multVec(_controls[i]));
136 
137  // add tangent points
138  for (SLuint i = 0; i < (SLuint)numControlPoints; ++i)
139  {
140  renderPoints.push_back(wm.multVec(_controls[i]));
141  int iPoint = (SLint)((SLfloat)i / 2.0f + 0.5f);
142  renderPoints.push_back(wm.multVec(_points[(SLuint)iPoint].vec3()));
143  }
144 
145  // Generate finally the OpenGL rendering buffer
146  _vao.generateVertexPos(&renderPoints);
147  }
148 
149  if (!_vao.vaoID()) return;
150 
151  // Set the view transform
152  SLGLState* stateGL = SLGLState::instance();
153  stateGL->modelMatrix.identity();
154 
155  SLint numTangentPoints = numControlPoints * 2;
156  SLint numCurvePoints = (SLint)_vao.numVertices() -
157  (SLint)_points.size() -
159  numTangentPoints;
160 
161  // Draw curve as a line strip through interpolated points
163  SLCol4f::RED,
164  1,
165  0,
166  (SLuint)numCurvePoints);
167 
168 // ES2 has often problems with rendering points
169 #ifndef APP_USES_GLES
170  // Draw curve as a line strip through interpolated points
172  SLCol4f::RED,
173  3,
174  0,
175  (SLuint)numCurvePoints);
176 
177  // Draw input points
180  6,
181  (SLuint)numCurvePoints,
182  (SLuint)_points.size());
183 
184  // Draw control points
187  6,
188  (SLuint)numCurvePoints + (SLuint)_points.size(),
190 
191  // Draw tangent points as lines
194  1,
195  (SLuint)numCurvePoints +
196  (SLuint)_points.size() +
198  (SLuint)numTangentPoints);
199 #endif
200 }
201 //-------------------------------------------------------------------------------
202 //! Deletes all curve arrays
204 {
205  _points.clear();
206  _lengths.clear();
207  _totalLength = 0.0f;
208 }
209 //-------------------------------------------------------------------------------
210 /*!
211 SLCurveBezier::curveEvaluate determines the position on the curve at time t.
212 */
214 {
215  assert(_points.size() > 1);
216 
217  // handle boundary conditions
218  if (t <= _points[0].w)
219  return _points[0].vec3();
220  else if (t >= _points.back().w)
221  return _points.back().vec3();
222 
223  // find segment and parameter
224  unsigned int i;
225  for (i = 0; i < _points.size() - 1; ++i)
226  if (t < _points[i + 1].w)
227  break;
228 
229  SLfloat t0 = _points[i].w;
230  SLfloat t1 = _points[i + 1].w;
231  SLfloat u = (t - t0) / (t1 - t0);
232 
233  // evaluate
234  SLVec3f A = _points[i + 1].vec3() -
235  3.0f * _controls[2 * i + 1] +
236  3.0f * _controls[2 * i] -
237  _points[i].vec3();
238  SLVec3f B = 3.0f * _controls[2 * i + 1] -
239  6.0f * _controls[2 * i] +
240  3.0f * _points[i].vec3();
241  SLVec3f C = 3.0f * _controls[2 * i] -
242  3.0f * _points[i].vec3();
243 
244  return _points[i].vec3() + u * (C + u * (B + u * A));
245 }
246 //-------------------------------------------------------------------------------
247 /*!
248 SLCurveBezier::curveVelocity determines the velocity vector on the curve at
249 point t. The velocity vector direction is the tangent vector at t an is the first
250 derivative at point t. The velocity vector magnitude is the speed at point t.
251 */
253 {
254  assert(_points.size() > 1);
255 
256  // handle boundary conditions
257  if (t <= _points[0].w)
258  return _points[0].vec3();
259  else if (t >= _points.back().w)
260  return _points.back().vec3();
261 
262  // find segment and parameter
263  unsigned int i;
264  for (i = 0; i < _points.size() - 1; ++i)
265  if (t < _points[i + 1].w)
266  break;
267 
268  SLfloat t0 = _points[i].w;
269  SLfloat t1 = _points[i + 1].w;
270  SLfloat u = (t - t0) / (t1 - t0);
271 
272  // evaluate
273  SLVec3f A = _points[i + 1].vec3() -
274  3.0f * _controls[2 * i + 1] +
275  3.0f * _controls[2 * i] -
276  _points[i].vec3();
277  SLVec3f B = 6.0f * _controls[2 * i + 1] -
278  12.0f * _controls[2 * i] +
279  6.0f * _points[i].vec3();
280  SLVec3f C = 3.0f * _controls[2 * i] -
281  3.0f * _points[i].vec3();
282 
283  return C + u * (B + 3.0f * u * A);
284 }
285 //-------------------------------------------------------------------------------
286 /*!
287 SLCurveBezier::curveAcceleration determines the acceleration vector on the curve
288 at time t. It is the second derivative at point t.
289 */
291 {
292  assert(_points.size() > 1);
293 
294  // handle boundary conditions
295  if (t <= _points[0].w)
296  return _points[0].vec3();
297  else if (t >= _points.back().w)
298  return _points.back().vec3();
299 
300  // find segment and parameter
301  unsigned int i;
302  for (i = 0; i < _points.size() - 1; ++i)
303  if (t < _points[i + 1].w) break;
304 
305  SLfloat t0 = _points[i].w;
306  SLfloat t1 = _points[i + 1].w;
307  SLfloat u = (t - t0) / (t1 - t0);
308 
309  // evaluate
310  SLVec3f A = _points[i + 1].vec3() -
311  3.0f * _controls[2 * i + 1] +
312  3.0f * _controls[2 * i] -
313  _points[i].vec3();
314  SLVec3f B = 6.0f * _controls[2 * i + 1] -
315  12.0f * _controls[2 * i] +
316  6.0f * _points[i].vec3();
317 
318  return B + 6.0f * u * A;
319 }
320 //-------------------------------------------------------------------------------
321 /*!
322 SLCurveBezier::findParamByDist gets parameter s distance in arc length from Q(t1).
323 Returns max SLfloat if can't find it.
324 */
326 {
327  // ensure that we remain within valid parameter space
328  if (s > arcLength(t1, _points.back().w))
329  return _points.back().w;
330 
331  // make first guess
332  SLfloat p = t1 + s * (_points.back().w - _points[0].w) / _totalLength;
333  for (SLuint i = 0; i < 32; ++i)
334  {
335  // compute function value and test against zero
336  SLfloat func = arcLength(t1, p) - s;
337  if (Utils::abs(func) < 1.0e-03f) return p;
338 
339  // perform Newton-Raphson iteration step
340  SLfloat speed = velocity(p).length();
341  assert(Utils::abs(speed) > FLT_EPSILON);
342  p -= func / speed;
343  }
344 
345  // done iterating, return failure case
346  return FLT_MAX;
347 }
348 //-------------------------------------------------------------------------------
349 /*!
350 Calculate length of curve between parameters t1 and t2
351 */
353 {
354  if (t2 <= t1) return 0.0f;
355  if (t1 < _points[0].w) t1 = _points[0].w;
356  if (t2 > _points.back().w) t2 = _points.back().w;
357 
358  // find segment and parameter
359  unsigned int seg1;
360  for (seg1 = 0; seg1 < _points.size() - 1; ++seg1)
361  if (t1 < _points[seg1 + 1].w)
362  break;
363 
364  SLfloat u1 = (t1 - _points[seg1].w) / (_points[seg1 + 1].w - _points[seg1].w);
365 
366  // find segment and parameter
367  unsigned int seg2;
368  for (seg2 = 0; seg2 < _points.size() - 1; ++seg2)
369  if (t2 <= _points[seg2 + 1].w)
370  break;
371 
372  SLfloat u2 = (t2 - _points[seg2].w) / (_points[seg2 + 1].w - _points[seg2].w);
373 
374  // both parameters lie in one segment
375  SLfloat result;
376  if (seg1 == seg2)
377  result = segmentArcLength(seg1, u1, u2);
378 
379  // parameters cross segments
380  else
381  {
382  result = segmentArcLength(seg1, u1, 1.0f);
383  for (SLuint i = seg1 + 1; i < seg2; ++i)
384  result += _lengths[i];
385  result += segmentArcLength(seg2, 0.0f, u2);
386  }
387 
388  return result;
389 }
390 //-------------------------------------------------------------------------------
391 /*!
392 SLCurveBezier::segmentArcLength calculate length of curve segment between
393 parameters u1 and u2 by recursively subdividing the segment.
394 */
396 {
397  assert(i >= 0 && i < _points.size() - 1);
398 
399  if (u2 <= u1) return 0.0f;
400  if (u1 < 0.0f) u1 = 0.0f;
401  if (u2 > 1.0f) u2 = 1.0f;
402 
403  SLVec3f P0 = _points[i].vec3();
404  SLVec3f P1 = _controls[2 * i];
405  SLVec3f P2 = _controls[2 * i + 1];
406  SLVec3f P3 = _points[i + 1].vec3();
407 
408  // get control points for subcurve from 0.0 to u2 (de Casteljau's method)
409  // http://de.wikipedia.org/wiki/De-Casteljau-Algorithmus
410  SLfloat minus_u2 = (1.0f - u2);
411  SLVec3f L1 = minus_u2 * P0 + u2 * P1;
412  SLVec3f H = minus_u2 * P1 + u2 * P2;
413  SLVec3f L2 = minus_u2 * L1 + u2 * H;
414  SLVec3f L3 = minus_u2 * L2 + u2 * (minus_u2 * H + u2 * (minus_u2 * P2 + u2 * P3));
415 
416  // resubdivide to get control points for subcurve between u1 and u2
417  SLfloat minus_u1 = (1.0f - u1);
418  H = minus_u1 * L1 + u1 * L2;
419  SLVec3f R3 = L3;
420  SLVec3f R2 = minus_u1 * L2 + u1 * L3;
421  SLVec3f R1 = minus_u1 * H + u1 * R2;
422  SLVec3f R0 = minus_u1 * (minus_u1 * (minus_u1 * P0 + u1 * L1) + u1 * H) + u1 * R1;
423 
424  // get length through subdivision
425  return subdivideLength(R0, R1, R2, R3);
426 }
427 //-------------------------------------------------------------------------------
428 /*!
429 SLCurveBezier::subdivideLength calculates length of Bezier curve by recursive
430 midpoint subdivision.
431 */
433  const SLVec3f& P1,
434  const SLVec3f& P2,
435  const SLVec3f& P3)
436 {
437  // check to see if basically straight
438  SLfloat Lmin = P0.distance(P3);
439  SLfloat Lmax = P0.distance(P1) + P1.distance(P2) + P2.distance(P3);
440  SLfloat diff = Lmin - Lmax;
441 
442  if (diff * diff < 1.0e-3f)
443  return 0.5f * (Lmin + Lmax);
444 
445  // otherwise get control points for subdivision
446  SLVec3f L1 = (P0 + P1) * 0.5f;
447  SLVec3f H = (P1 + P2) * 0.5f;
448  SLVec3f L2 = (L1 + H) * 0.5f;
449  SLVec3f R2 = (P2 + P3) * 0.5f;
450  SLVec3f R1 = (H + R2) * 0.5f;
451  SLVec3f mid = (L2 + R1) * 0.5f;
452 
453  // subdivide
454  return subdivideLength(P0, L1, L2, mid) + subdivideLength(mid, R1, R2, P3);
455 }
456 //-------------------------------------------------------------------------------
457 /*!
458 SLCurveBezier::subdivideRender adds points along the curve to the point vector
459 renderPoints by recursively subdividing the curve with the Casteljau scheme.
460 */
462  const SLMat4f& wm,
463  SLfloat epsilon,
464  const SLVec3f& P0,
465  const SLVec3f& P1,
466  const SLVec3f& P2,
467  const SLVec3f& P3)
468 {
469  // add first point transformed by wm if not already in the list
470  if (renderPoints.empty())
471  renderPoints.push_back(wm.multVec(P0));
472  else if (P0 != renderPoints.back())
473  renderPoints.push_back(wm.multVec(P0));
474 
475  // check to see if basically straight
476  SLfloat Lmin = P0.distance(P3);
477  SLfloat Lmax = P0.distance(P1) + P1.distance(P2) + P2.distance(P3);
478  SLfloat diff = Lmin - Lmax;
479  if (diff * diff < epsilon) return;
480 
481  // otherwise get control points for subdivision
482  SLVec3f L1 = (P0 + P1) * 0.5f;
483  SLVec3f H = (P1 + P2) * 0.5f;
484  SLVec3f L2 = (L1 + H) * 0.5f;
485  SLVec3f R2 = (P2 + P3) * 0.5f;
486  SLVec3f R1 = (H + R2) * 0.5f;
487  SLVec3f mid = (L2 + R1) * 0.5f;
488 
489  // subdivide
490  subdivideRender(renderPoints, wm, epsilon, P0, L1, L2, mid);
491  subdivideRender(renderPoints, wm, epsilon, mid, R1, R2, P3);
492 }
493 //-------------------------------------------------------------------------------
float SLfloat
Definition: SL.h:173
unsigned int SLuint
Definition: SL.h:171
int SLint
Definition: SL.h:170
@ PT_points
Definition: SLGLEnums.h:31
@ PT_lines
Definition: SLGLEnums.h:32
@ PT_lineStrip
Definition: SLGLEnums.h:34
Singleton class for global render state.
SLScene * s
Definition: SLScene.h:31
vector< SLVec3f > SLVVec3f
Definition: SLVec3.h:325
vector< SLVec4f > SLVVec4f
Definition: SLVec4.h:239
SLVec4< SLfloat > SLVec4f
Definition: SLVec4.h:235
void subdivideRender(SLVVec3f &points, const SLMat4f &wm, SLfloat epsilon, const SLVec3f &P0, const SLVec3f &P1, const SLVec3f &P2, const SLVec3f &P3)
SLVVec3f _controls
Control points of Bezier curve.
Definition: SLCurveBezier.h:59
void init(const SLVVec4f &points, const SLVVec3f &controlPoints)
SLVec3f velocity(SLfloat t)
SLint numControlPoints()
Definition: SLCurveBezier.h:55
SLVec3f acceleration(SLfloat t)
SLGLVertexArrayExt _vao
Vertex array object for rendering.
Definition: SLCurveBezier.h:60
SLfloat arcLength(SLfloat t1, SLfloat t2)
SLCurveBezier(const SLVVec4f &points)
SLVec3f evaluate(const SLfloat t)
SLfloat segmentArcLength(SLuint i, SLfloat u1, SLfloat u2)
void draw(const SLMat4f &wm)
void dispose()
Deletes all curve arrays.
SLfloat subdivideLength(const SLVec3f &P0, const SLVec3f &P1, const SLVec3f &P2, const SLVec3f &P3)
SLfloat findParamByDist(SLfloat t1, SLfloat s)
SLVVec4f _points
Sample points (x,y,z) and time (w) of curve.
Definition: SLCurve.h:34
SLVfloat _lengths
Length of each curve segment.
Definition: SLCurve.h:35
SLfloat _totalLength
Total length of curve.
Definition: SLCurve.h:36
Singleton class holding all OpenGL states.
Definition: SLGLState.h:71
SLMat4f modelMatrix
Init all states.
Definition: SLGLState.h:89
static SLGLState * instance()
Public static instance getter for singleton pattern.
Definition: SLGLState.h:74
void generateVertexPos(SLVVec2f *p)
Adds or updates & generates a position vertex attribute for colored line or point drawing.
void drawArrayAsColored(SLGLPrimitiveType primitiveType, SLCol4f color, SLfloat lineOrPointSize=1.0f, SLuint indexFirstVertex=0, SLuint countVertices=0)
Draws the array as the specified primitive with the color.
SLuint numVertices() const
SLuint vaoID() const
Returns either the VAO id or the VBO id.
SLVec3< T > multVec(SLVec3< T > v) const
Definition: SLMat4.h:576
void identity()
Sets the identity matrix.
Definition: SLMat4.h:1333
The SLScene class represents the top level instance holding the scene structure.
Definition: SLScene.h:47
T length() const
Definition: SLVec3.h:122
T distance(const SLVec3 &p) const
Calculate the distance to point p.
Definition: SLVec3.h:168
static SLVec4 YELLOW
Definition: SLVec4.h:219
static SLVec4 RED
Definition: SLVec4.h:216
static SLVec4 BLUE
Definition: SLVec4.h:218
T abs(T a)
Definition: Utils.h:249