libmoldeo (Moldeo 1.0 Core)  1.0
libmoldeo is the group of objects and functions that executes the basic operations of Moldeo 1.0 Platform.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
moMathCurve.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 
3  moMathCurve.cpp
4 
5  ****************************************************************************
6  * *
7  * This source is free software; you can redistribute it and/or modify *
8  * it under the terms of the GNU General Public License as published by *
9  * the Free Software Foundation; either version 2 of the License, or *
10  * (at your option) any later version. *
11  * *
12  * This code is distributed in the hope that it will be useful, but *
13  * WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
15  * General Public License for more details. *
16  * *
17  * A copy of the GNU General Public License is available on the World *
18  * Wide Web at <http://www.gnu.org/copyleft/gpl.html>. You can also *
19  * obtain it by writing to the Free Software Foundation, *
20  * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
21  * *
22  ****************************************************************************
23 
24  Copyright(C) 2006 Fabricio Costa
25 
26  Authors:
27  Fabricio Costa
28  Andrés Colubri
29 
30  Portions taken from
31  Wild Magic Source Code
32  David Eberly
33  http://www.geometrictools.com
34  Copyright (c) 1998-2007
35 
36 *******************************************************************************/
37 
38 #include "moMathCurve.h"
39 
40 // moCurve2 class ------------------------------------------------------------
41 /*
42 template <class Real>
43 moCurve2<Real>::moCurve2 (Real fTMin, Real fTMax)
44 {
45  m_fTMin = fTMin;
46  m_fTMax = fTMax;
47 }
48 
49 template <class Real>
50 moCurve2<Real>::~moCurve2 ()
51 {
52 }
53 
54 template <class Real>
55 Real moCurve2<Real>::GetMinTime () const
56 {
57  return m_fTMin;
58 }
59 
60 template <class Real>
61 Real moCurve2<Real>::GetMaxTime () const
62 {
63  return m_fTMax;
64 }
65 
66 template <class Real>
67 void moCurve2<Real>::SetTimeInterval (Real fTMin, Real fTMax)
68 {
69  m_fTMin = fTMin;
70  m_fTMax = fTMax;
71 }
72 
73 template <class Real>
74 Real moCurve2<Real>::GetSpeed (Real fTime) const
75 {
76  moVector2<Real> kVelocity = GetFirstDerivative(fTime);
77  Real fSpeed = kVelocity.Length();
78  return fSpeed;
79 }
80 
81 template <class Real>
82 Real moCurve2<Real>::GetTotalLength () const
83 {
84  return GetLength(m_fTMin,m_fTMax);
85 }
86 
87 template <class Real>
88 moVector2<Real> moCurve2<Real>::GetTangent (Real fTime) const
89 {
90  moVector2<Real> kVelocity = GetFirstDerivative(fTime);
91  kVelocity.Normalize();
92  return kVelocity;
93 }
94 
95 template <class Real>
96 moVector2<Real> moCurve2<Real>::GetNormal (Real fTime) const
97 {
98  moVector2<Real> kTangent = GetFirstDerivative(fTime);
99  kTangent.Normalize();
100  moVector2<Real> kNormal = kTangent.Perp();
101  return kNormal;
102 }
103 
104 template <class Real>
105 void moCurve2<Real>::GetFrame (Real fTime, moVector2<Real>& rkPosition,
106  moVector2<Real>& rkTangent, moVector2<Real>& rkNormal) const
107 {
108  rkPosition = GetPosition(fTime);
109  rkTangent = GetFirstDerivative(fTime);
110  rkTangent.Normalize();
111  rkNormal = rkTangent.Perp();
112 }
113 
114 template <class Real>
115 Real moCurve2<Real>::GetCurvature (Real fTime) const
116 {
117  moVector2<Real> kDer1 = GetFirstDerivative(fTime);
118  moVector2<Real> kDer2 = GetSecondDerivative(fTime);
119  Real fSpeedSqr = kDer1.SquaredLength();
120 
121  if (fSpeedSqr >= moMath<Real>::ZERO_TOLERANCE)
122  {
123  Real fNumer = kDer1.DotPerp(kDer2);
124  Real fDenom = moMath<Real>::Pow(fSpeedSqr,(Real)1.5);
125  return fNumer/fDenom;
126  }
127  else
128  {
129  // curvature is indeterminate, just return 0
130  return (Real)0.0;
131  }
132 }
133 
134 template <class Real>
135 void moCurve2<Real>::SubdivideByTime (int iNumPoints,
136  moVector2<Real>*& rakPoint) const
137 {
138  rakPoint = new moVector2<Real>[iNumPoints];
139 
140  Real fDelta = (m_fTMax - m_fTMin)/(iNumPoints-1);
141 
142  for (int i = 0; i < iNumPoints; i++)
143  {
144  Real fTime = m_fTMin + fDelta*i;
145  rakPoint[i] = GetPosition(fTime);
146  }
147 }
148 
149 template <class Real>
150 void moCurve2<Real>::SubdivideByLength (int iNumPoints,
151  moVector2<Real>*& rakPoint) const
152 {
153  rakPoint = new moVector2<Real>[iNumPoints];
154 
155  Real fDelta = GetTotalLength()/(iNumPoints-1);
156 
157  for (int i = 0; i < iNumPoints; i++)
158  {
159  Real fLength = fDelta*i;
160  Real fTime = GetTime(fLength);
161  rakPoint[i] = GetPosition(fTime);
162  }
163 }
164 
165 template <class Real>
166 void moCurve2<Real>::SubdivideByVariation (Real fT0, const moVector2<Real>& rkP0,
167  Real fT1, const moVector2<Real>& rkP1, Real fMinVariation,
168  int iLevel, int& riNumPoints, PointList*& rpkList) const
169 {
170  if (iLevel > 0 && GetVariation(fT0,fT1,&rkP0,&rkP1) > fMinVariation)
171  {
172  // too much variation, subdivide interval
173  iLevel--;
174  Real fTMid = ((Real)0.5)*(fT0+fT1);
175  moVector2<Real> kPMid = GetPosition(fTMid);
176 
177  SubdivideByVariation(fT0,rkP0,fTMid,kPMid,fMinVariation,iLevel,
178  riNumPoints,rpkList);
179 
180  SubdivideByVariation(fTMid,kPMid,fT1,rkP1,fMinVariation,iLevel,
181  riNumPoints,rpkList);
182  }
183  else
184  {
185  // add right end point, left end point was added by neighbor
186  rpkList = new PointList(rkP1,rpkList);
187  riNumPoints++;
188  }
189 }
190 
191 template <class Real>
192 void moCurve2<Real>::SubdivideByVariation (Real fMinVariation, int iMaxLevel,
193  int& riNumPoints, moVector2<Real>*& rakPoint) const
194 {
195  // compute end points of curve
196  moVector2<Real> kPMin = GetPosition(m_fTMin);
197  moVector2<Real> kPMax = GetPosition(m_fTMax);
198 
199  // add left end point to list
200  PointList* pkList = new PointList(kPMin,0);
201  riNumPoints = 1;
202 
203  // binary subdivision, leaf nodes add right end point of subinterval
204  SubdivideByVariation(m_fTMin,kPMin,m_fTMax,kPMax,fMinVariation,
205  iMaxLevel,riNumPoints,pkList->m_kNext);
206 
207  // repackage points in an array
208  rakPoint = new moVector2<Real>[riNumPoints];
209  for (int i = 0; i < riNumPoints; i++)
210  {
211  rakPoint[i] = pkList->m_kPoint;
212 
213  PointList* pkSave = pkList;
214  pkList = pkList->m_kNext;
215  delete pkSave;
216  }
217 }
218 */
219 // moSingleCurve2 class ------------------------------------------------------------
220 /*
221 template <class Real>
222 moSingleCurve2<Real>::moSingleCurve2 (Real fTMin, Real fTMax)
223  :
224  moCurve2<Real>(fTMin,fTMax)
225 {
226 }
227 
228 template <class Real>
229 Real moSingleCurve2<Real>::GetSpeedWithData (Real fTime, void* pvData)
230 {
231  return ((moCurve2<Real>*)pvData)->GetSpeed(fTime);
232 }
233 
234 template <class Real>
235 Real moSingleCurve2<Real>::GetLength (Real fT0, Real fT1) const
236 {
240 
241  return moIntegrate1<Real>::RombergIntegral(8,fT0,fT1,GetSpeedWithData,
242  (void*)this);
243 }
244 
245 template <class Real>
246 Real moSingleCurve2<Real>::GetTime (Real fLength, int iIterations,
247  Real fTolerance) const
248 {
249  if (fLength <= (Real)0.0)
250  {
251  return m_fTMin;
252  }
253 
254  if (fLength >= GetTotalLength())
255  {
256  return m_fTMax;
257  }
258 
259  // initial guess for Newton's method
260  Real fRatio = fLength/GetTotalLength();
261  Real fOmRatio = ((Real)1.0) - fRatio;
262  Real fTime = fOmRatio*m_fTMin + fRatio*m_fTMax;
263 
264  for (int i = 0; i < iIterations; i++)
265  {
266  Real fDifference = GetLength(m_fTMin,fTime) - fLength;
267  if (moMath<Real>::FAbs(fDifference) < fTolerance)
268  {
269  return fTime;
270  }
271 
272  fTime -= fDifference/GetSpeed(fTime);
273  }
274 
275  // Newton's method failed. You might want to increase iterations or
276  // tolerance or integration accuracy. However, in this application it
277  // is likely that the time values are oscillating, due to the limited
278  // numerical precision of 32-bit floats. It is safe to use the last
279  // computed time.
280  return fTime;
281 }
282 */
283 // moMultipleCurve2 ------------------------------------------------------------
284 /*
285 template <class Real>
286 moMultipleCurve2<Real>::moMultipleCurve2 (int iSegments, Real* afTime)
287  :
288  moCurve2<Real>(afTime[0],afTime[iSegments])
289 {
290  m_iSegments = iSegments;
291  m_afTime = afTime;
292  m_afLength = 0;
293  m_afAccumLength = 0;
294 }
295 
296 template <class Real>
297 moMultipleCurve2<Real>::~moMultipleCurve2 ()
298 {
299  delete[] m_afTime;
300  delete[] m_afLength;
301  delete[] m_afAccumLength;
302 }
303 
304 template <class Real>
305 int moMultipleCurve2<Real>::GetSegments () const
306 {
307  return m_iSegments;
308 }
309 
310 template <class Real>
311 const Real* moMultipleCurve2<Real>::GetTimes () const
312 {
313  return m_afTime;
314 }
315 
316 template <class Real>
317 void moMultipleCurve2<Real>::GetKeyInfo (Real fTime, int& riKey, Real& rfDt)
318  const
319 {
320  if (fTime <= m_afTime[0])
321  {
322  riKey = 0;
323  rfDt = (Real)0.0;
324  }
325  else if (fTime >= m_afTime[m_iSegments])
326  {
327  riKey = m_iSegments-1;
328  rfDt = m_afTime[m_iSegments] - m_afTime[m_iSegments-1];
329  }
330  else
331  {
332  for (int i = 0; i < m_iSegments; i++)
333  {
334  if (fTime < m_afTime[i+1])
335  {
336  riKey = i;
337  rfDt = fTime - m_afTime[i];
338  break;
339  }
340  }
341  }
342 }
343 
344 template <class Real>
345 Real moMultipleCurve2<Real>::GetSpeedWithData (Real fTime, void* pvData)
346 {
347  moMultipleCurve2* pvThis = *(moMultipleCurve2**) pvData;
348  int iKey = *(int*)((char*)pvData + sizeof(pvThis));
349  return pvThis->GetSpeedKey(iKey,fTime);
350 }
351 
352 template <class Real>
353 void moMultipleCurve2<Real>::InitializeLength () const
354 {
355  m_afLength = new Real[m_iSegments];
356  m_afAccumLength = new Real[m_iSegments];
357 
358  // arc lengths of the segments
359  int iKey;
360  for (iKey = 0; iKey < m_iSegments; iKey++)
361  {
362  m_afLength[iKey] = GetLengthKey(iKey,(Real)0.0,
363  m_afTime[iKey+1]-m_afTime[iKey]);
364  }
365 
366  // accumulative arc length
367  m_afAccumLength[0] = m_afLength[0];
368  for (iKey = 1; iKey < m_iSegments; iKey++)
369  {
370  m_afAccumLength[iKey] = m_afAccumLength[iKey-1] + m_afLength[iKey];
371  }
372 }
373 
374 template <class Real>
375 Real moMultipleCurve2<Real>::GetLength (Real fT0, Real fT1) const
376 {
377  //assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
378  //assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
379  //assert(fT0 <= fT1);
380 
381  if (!m_afLength)
382  {
383  InitializeLength();
384  }
385 
386  int iKey0, iKey1;
387  Real fDt0, fDt1;
388  GetKeyInfo(fT0,iKey0,fDt0);
389  GetKeyInfo(fT1,iKey1,fDt1);
390 
391  Real fLength;
392  if (iKey0 < iKey1)
393  {
394  // accumulate full-segment lengths
395  fLength = (Real)0.0;
396  for (int i = iKey0+1; i < iKey1; i++)
397  {
398  fLength += m_afLength[i];
399  }
400 
401  // add on partial first segment
402  fLength += GetLengthKey(iKey0,fDt0,m_afTime[iKey0+1]-m_afTime[iKey0]);
403 
404  // add on partial last segment
405  fLength += GetLengthKey(iKey1,(Real)0.0,fDt1);
406  }
407  else
408  {
409  fLength = GetLengthKey(iKey0,fDt0,fDt1);
410  }
411 
412  return fLength;
413 }
414 
415 template <class Real>
416 Real moMultipleCurve2<Real>::GetTime (Real fLength, int iIterations,
417  Real fTolerance) const
418 {
419  if (!m_afLength)
420  {
421  InitializeLength();
422  }
423 
424  if (fLength <= (Real)0.0)
425  {
426  return m_fTMin;
427  }
428 
429  if (fLength >= m_afAccumLength[m_iSegments-1])
430  {
431  return m_fTMax;
432  }
433 
434  int iKey;
435  for (iKey = 0; iKey < m_iSegments; iKey++)
436  {
437  if (fLength < m_afAccumLength[iKey])
438  {
439  break;
440  }
441  }
442  if (iKey >= m_iSegments)
443  {
444  return m_afTime[m_iSegments];
445  }
446 
447  // try Newton's method first for rapid convergence
448  Real fL0, fL1;
449  if (iKey == 0)
450  {
451  fL0 = fLength;
452  fL1 = m_afAccumLength[0];
453  }
454  else
455  {
456  fL0 = fLength - m_afAccumLength[iKey-1];
457  fL1 = m_afAccumLength[iKey] - m_afAccumLength[iKey-1];
458  }
459 
460  // use Newton's method to invert the arc length integral
461  Real fDt1 = m_afTime[iKey+1] - m_afTime[iKey];
462  Real fDt0 = fDt1*fL0/fL1;
463  for (int i = 0; i < iIterations; i++)
464  {
465  Real fDifference = GetLengthKey(iKey,(Real)0.0,fDt0) - fL0;
466  if (moMath<Real>::FAbs(fDifference) <= fTolerance)
467  {
468  return m_afTime[iKey] + fDt0;
469  }
470 
471  fDt0 -= fDifference/GetSpeedKey(iKey,fDt0);
472  }
473 
474  // Newton's method failed. You might want to increase iterations or
475  // tolerance or integration accuracy. However, in this application it
476  // is likely that the time values are oscillating, due to the limited
477  // numerical precision of 32-bit floats. It is safe to use the last
478  // computed time.
479  return m_afTime[iKey] + fDt0;
480 }
481 
482 template <class Real>
483 Real moMultipleCurve2<Real>::GetVariation (Real fT0, Real fT1,
484  const moVector2<Real>* pkP0, const moVector2<Real>* pkP1) const
485 {
486  //assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
487  //assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
488  //assert(fT0 <= fT1);
489 
490  // construct line segment, A + (t-t0)*B
491  moVector2<Real> kP0, kP1;
492  if (!pkP0)
493  {
494  kP0 = GetPosition(fT0);
495  pkP0 = &kP0;
496  }
497  if (!pkP1)
498  {
499  kP1 = GetPosition(fT1);
500  pkP1 = &kP1;
501  }
502  Real fInvDT = ((Real)1.0)/(fT1 - fT0);
503  moVector2<Real> kA, kB = fInvDT*(*pkP1 - *pkP0);
504 
505  int iKey0, iKey1;
506  Real fDt0, fDt1;
507  GetKeyInfo(fT0,iKey0,fDt0);
508  GetKeyInfo(fT1,iKey1,fDt1);
509 
510  Real fVariation;
511  if (iKey0 < iKey1)
512  {
513  // accumulate full-segment variations
514  fVariation = (Real)0.0;
515  for (int i = iKey0+1; i < iKey1; i++)
516  {
517  kA = *pkP0 + (m_afTime[i] - fT0)*kB;
518  fVariation += GetVariationKey(i,(Real)0.0,
519  m_afTime[i+1]-m_afTime[i],kA,kB);
520  }
521 
522  // add on partial first segment
523  kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
524  fVariation += GetVariationKey(iKey0,fDt0,
525  m_afTime[iKey0+1]-m_afTime[iKey0],kA,kB);
526 
527  // add on partial last segment
528  kA = *pkP0 + (m_afTime[iKey1] - fT0)*kB;
529  fVariation += GetVariationKey(iKey1,(Real)0.0,fDt1,kA,kB);
530  }
531  else
532  {
533  kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
534  fVariation = GetVariationKey(iKey0,fDt0,fDt1,kA,kB);
535  }
536 
537  return fVariation;
538 }
539 */
540 // moCurve3 class ------------------------------------------------------------
541 /*
542 template <class Real>
543 Real moCurve3<Real>::GetMinTime () const
544 {
545  return m_fTMin;
546 }
547 
548 template <class Real>
549 Real moCurve3<Real>::GetMaxTime () const
550 {
551  return m_fTMax;
552 }
553 
554 template <class Real>
555 void moCurve3<Real>::SetTimeInterval (Real fTMin, Real fTMax)
556 {
557  //assert(fTMin < fTMax);
558  m_fTMin = fTMin;
559  m_fTMax = fTMax;
560 }
561 
562 template <class Real>
563 Real moCurve3<Real>::GetSpeed (Real fTime) const
564 {
565  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
566  Real fSpeed = kVelocity.Length();
567  return fSpeed;
568 }
569 
570 template <class Real>
571 Real moCurve3<Real>::GetTotalLength () const
572 {
573  return GetLength(m_fTMin,m_fTMax);
574 }
575 
576 template <class Real>
577 moVector3<Real> moCurve3<Real>::GetTangent (Real fTime) const
578 {
579  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
580  kVelocity.Normalize();
581  return kVelocity;
582 }
583 
584 template <class Real>
585 moVector3<Real> moCurve3<Real>::GetNormal (Real fTime) const
586 {
587  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
588  moVector3<Real> kAcceleration = GetSecondDerivative(fTime);
589  Real fVDotV = kVelocity.Dot(kVelocity);
590  Real fVDotA = kVelocity.Dot(kAcceleration);
591  moVector3<Real> kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
592  kNormal.Normalize();
593  return kNormal;
594 }
595 */
596 /*
597 template <class Real>
598 moVector3<Real> moCurve3<Real>::GetBinormal (Real fTime) const
599 {
600  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
601  moVector3<Real> kAcceleration = GetSecondDerivative(fTime);
602  Real fVDotV = kVelocity.Dot(kVelocity);
603  Real fVDotA = kVelocity.Dot(kAcceleration);
604  moVector3<Real> kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
605  kNormal.Normalize();
606  kVelocity.Normalize();
607  moVector3<Real> kBinormal = kVelocity.Cross(kNormal);
608  return kBinormal;
609 }
610 
611 template <class Real>
612 void moCurve3<Real>::GetFrame (Real fTime, moVector3<Real>& rkPosition,
613  moVector3<Real>& rkTangent, moVector3<Real>& rkNormal,
614  moVector3<Real>& rkBinormal) const
615 {
616  rkPosition = GetPosition(fTime);
617  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
618  moVector3<Real> kAcceleration = GetSecondDerivative(fTime);
619  Real fVDotV = kVelocity.Dot(kVelocity);
620  Real fVDotA = kVelocity.Dot(kAcceleration);
621  rkNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
622  rkNormal.Normalize();
623  rkTangent = kVelocity;
624  rkTangent.Normalize();
625  rkBinormal = rkTangent.Cross(rkNormal);
626 }
627 
628 template <class Real>
629 Real moCurve3<Real>::GetCurvature (Real fTime) const
630 {
631  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
632  Real fSpeedSqr = kVelocity.SquaredLength();
633 
634  if (fSpeedSqr >= moMath<Real>::ZERO_TOLERANCE)
635  {
636  moVector3<Real> kAcceleration = GetSecondDerivative(fTime);
637  moVector3<Real> kCross = kVelocity.Cross(kAcceleration);
638  Real fNumer = kCross.Length();
639  Real fDenom = moMath<Real>::Pow(fSpeedSqr,(Real)1.5);
640  return fNumer/fDenom;
641  }
642  else
643  {
644  // curvature is indeterminate, just return 0
645  return (Real)0.0;
646  }
647 }
648 
649 template <class Real>
650 Real moCurve3<Real>::GetTorsion (Real fTime) const
651 {
652  moVector3<Real> kVelocity = GetFirstDerivative(fTime);
653  moVector3<Real> kAcceleration = GetSecondDerivative(fTime);
654  moVector3<Real> kCross = kVelocity.Cross(kAcceleration);
655  Real fDenom = kCross.SquaredLength();
656 
657  if (fDenom >= moMath<Real>::ZERO_TOLERANCE)
658  {
659  moVector3<Real> kJerk = GetThirdDerivative(fTime);
660  Real fNumer = kCross.Dot(kJerk);
661  return fNumer/fDenom;
662  }
663  else
664  {
665  // torsion is indeterminate, just return 0
666  return (Real)0.0;
667  }
668 }
669 
670 template <class Real>
671 void moCurve3<Real>::SubdivideByTime (int iNumPoints,
672  moVector3<Real>*& rakPoint) const
673 {
674  //assert( iNumPoints >= 2 );
675  rakPoint = new moVector3<Real>[iNumPoints];
676 
677  Real fDelta = (m_fTMax - m_fTMin)/(iNumPoints-1);
678 
679  for (int i = 0; i < iNumPoints; i++)
680  {
681  Real fTime = m_fTMin + fDelta*i;
682  rakPoint[i] = GetPosition(fTime);
683  }
684 }
685 
686 template <class Real>
687 void moCurve3<Real>::SubdivideByLength (int iNumPoints,
688  moVector3<Real>*& rakPoint) const
689 {
690  //assert(iNumPoints >= 2);
691  rakPoint = new moVector3<Real>[iNumPoints];
692 
693  Real fDelta = GetTotalLength()/(iNumPoints-1);
694 
695  for (int i = 0; i < iNumPoints; i++)
696  {
697  Real fLength = fDelta*i;
698  Real fTime = GetTime(fLength);
699  rakPoint[i] = GetPosition(fTime);
700  }
701 }
702 
703 template <class Real>
704 void moCurve3<Real>::SubdivideByVariation (Real fT0, const moVector3<Real>& rkP0,
705  Real fT1, const moVector3<Real>& rkP1, Real fMinVariation, int iLevel,
706  int& riNumPoints, PointList*& rpkList) const
707 {
708  if (iLevel > 0 && GetVariation(fT0,fT1,&rkP0,&rkP1) > fMinVariation)
709  {
710  // too much variation, subdivide interval
711  iLevel--;
712  Real fTMid = ((Real)0.5)*(fT0+fT1);
713  moVector3<Real> kPMid = GetPosition(fTMid);
714 
715  SubdivideByVariation(fT0,rkP0,fTMid,kPMid,fMinVariation,iLevel,
716  riNumPoints,rpkList);
717 
718  SubdivideByVariation(fTMid,kPMid,fT1,rkP1,fMinVariation,iLevel,
719  riNumPoints,rpkList);
720  }
721  else
722  {
723  // add right end point, left end point was added by neighbor
724  rpkList = new PointList(rkP1,rpkList);
725  riNumPoints++;
726  }
727 }
728 
729 template <class Real>
730 void moCurve3<Real>::SubdivideByVariation (Real fMinVariation, int iMaxLevel,
731  int& riNumPoints, moVector3<Real>*& rakPoint) const
732 {
733  // compute end points of curve
734  moVector3<Real> kPMin = GetPosition(m_fTMin);
735  moVector3<Real> kPMax = GetPosition(m_fTMax);
736 
737  // add left end point to list
738  PointList* pkList = new PointList(kPMin,0);
739  riNumPoints = 1;
740 
741  // binary subdivision, leaf nodes add right end point of subinterval
742  SubdivideByVariation(m_fTMin,kPMin,m_fTMax,kPMax,fMinVariation,
743  iMaxLevel,riNumPoints,pkList->m_kNext);
744 
745  // repackage points in an array
746  //assert(riNumPoints >= 2);
747  rakPoint = new moVector3<Real>[riNumPoints];
748  for (int i = 0; i < riNumPoints; i++)
749  {
750  //assert(pkList);
751  rakPoint[i] = pkList->m_kPoint;
752 
753  PointList* pkSave = pkList;
754  pkList = pkList->m_kNext;
755  delete pkSave;
756  }
757  //assert(pkList == 0);
758 }
759 */
760 // moSingleCurve3 ------------------------------------------------------------
761 /*
762 template <class Real>
763 moSingleCurve3<Real>::moSingleCurve3 (Real fTMin, Real fTMax)
764  :
765  moCurve3<Real>(fTMin,fTMax)
766 {
767 }
768 
769 template <class Real>
770 Real moSingleCurve3<Real>::GetSpeedWithData (Real fTime, void* pvData)
771 {
772  return ((moCurve3<Real>*)pvData)->GetSpeed(fTime);
773 }
774 */
775 /*
776 template <class Real>
777 Real moSingleCurve3<Real>::GetLength (Real fT0, Real fT1) const
778 {
779  //assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
780  //assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
781  //assert(fT0 <= fT1);
782 
783  return moIntegrate1<Real>::RombergIntegral(8,fT0,fT1,GetSpeedWithData,
784  (void*)this);
785 }
786 
787 template <class Real>
788 Real moSingleCurve3<Real>::GetTime (Real fLength, int iIterations,
789  Real fTolerance) const
790 {
791  if (fLength <= (Real)0.0)
792  {
793  return m_fTMin;
794  }
795 
796  if (fLength >= GetTotalLength())
797  {
798  return m_fTMax;
799  }
800 
801  // initial guess for Newton's method
802  Real fRatio = fLength/GetTotalLength();
803  Real fOmRatio = (Real)1.0 - fRatio;
804  Real fTime = fOmRatio*m_fTMin + fRatio*m_fTMax;
805 
806  for (int i = 0; i < iIterations; i++)
807  {
808  Real fDifference = GetLength(m_fTMin,fTime) - fLength;
809  if (moMath<Real>::FAbs(fDifference) < fTolerance)
810  {
811  return fTime;
812  }
813 
814  fTime -= fDifference/GetSpeed(fTime);
815  }
816 
817  // Newton's method failed. You might want to increase iterations or
818  // tolerance or integration accuracy. However, in this application it
819  // is likely that the time values are oscillating, due to the limited
820  // numerical precision of 32-bit floats. It is safe to use the last
821  // computed time.
822  return fTime;
823 }
824 */
825 // moMultipleCurve3 ------------------------------------------------------------
826 /*
827 template <class Real>
828 moMultipleCurve3<Real>::moMultipleCurve3 (int iSegments, Real* afTime)
829  :
830  moCurve3<Real>(afTime[0],afTime[iSegments])
831 {
832  m_iSegments = iSegments;
833  m_afTime = afTime;
834  m_afLength = 0;
835  m_afAccumLength = 0;
836 }
837 
838 template <class Real>
839 moMultipleCurve3<Real>::~moMultipleCurve3 ()
840 {
841  delete[] m_afTime;
842  delete[] m_afLength;
843  delete[] m_afAccumLength;
844 }
845 */
846 /*
847 template <class Real>
848 int moMultipleCurve3<Real>::GetSegments () const
849 {
850  return m_iSegments;
851 }
852 
853 template <class Real>
854 const Real* moMultipleCurve3<Real>::GetTimes () const
855 {
856  return m_afTime;
857 }*/
858 /*
859 template <class Real>
860 void moMultipleCurve3<Real>::GetKeyInfo (Real fTime, int& riKey, Real& rfDt)
861  const
862 {
863  if (fTime <= m_afTime[0])
864  {
865  riKey = 0;
866  rfDt = (Real)0.0;
867  }
868  else if (fTime >= m_afTime[m_iSegments])
869  {
870  riKey = m_iSegments-1;
871  rfDt = m_afTime[m_iSegments] - m_afTime[m_iSegments-1];
872  }
873  else
874  {
875  for (int i = 0; i < m_iSegments; i++)
876  {
877  if (fTime < m_afTime[i+1])
878  {
879  riKey = i;
880  rfDt = fTime - m_afTime[i];
881  break;
882  }
883  }
884  }
885 }
886 */
887 /*
888 template <class Real>
889 Real moMultipleCurve3<Real>::GetSpeedWithData (Real fTime, void* pvData)
890 {
891  moMultipleCurve3* pvThis = *(moMultipleCurve3**) pvData;
892  int iKey = *(int*)((char*)pvData + sizeof(pvThis));
893  return pvThis->GetSpeedKey(iKey,fTime);
894 }
895 */
896 /*
897 template <class Real>
898 void moMultipleCurve3<Real>::InitializeLength () const
899 {
900  m_afLength = new Real[m_iSegments];
901  m_afAccumLength = new Real[m_iSegments];
902 
903  // arc lengths of the segments
904  int iKey;
905  for (iKey = 0; iKey < m_iSegments; iKey++)
906  {
907  m_afLength[iKey] = GetLengthKey(iKey,(Real)0.0,
908  m_afTime[iKey+1]-m_afTime[iKey]);
909  }
910 
911  // accumulative arc length
912  m_afAccumLength[0] = m_afLength[0];
913  for (iKey = 1; iKey < m_iSegments; iKey++)
914  {
915  m_afAccumLength[iKey] = m_afAccumLength[iKey-1] + m_afLength[iKey];
916  }
917 }
918 */
919 /*
920 template <class Real>
921 Real moMultipleCurve3<Real>::GetLength (Real fT0, Real fT1) const
922 {
923  //assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
924  //assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
925  //assert(fT0 <= fT1);
926 
927  if (!m_afLength)
928  {
929  InitializeLength();
930  }
931 
932  int iKey0, iKey1;
933  Real fDt0, fDt1;
934  GetKeyInfo(fT0,iKey0,fDt0);
935  GetKeyInfo(fT1,iKey1,fDt1);
936 
937  Real fLength;
938  if (iKey0 < iKey1)
939  {
940  // accumulate full-segment lengths
941  fLength = (Real)0.0;
942  for (int i = iKey0+1; i < iKey1; i++)
943  fLength += m_afLength[i];
944 
945  // add on partial first segment
946  fLength += GetLengthKey(iKey0,fDt0,m_afTime[iKey0+1]-m_afTime[iKey0]);
947 
948  // add on partial last segment
949  fLength += GetLengthKey(iKey1,(Real)0.0,fDt1);
950  }
951  else
952  {
953  fLength = GetLengthKey(iKey0,fDt0,fDt1);
954  }
955 
956  return fLength;
957 }
958 */
959 /*
960 template <class Real>
961 Real moMultipleCurve3<Real>::GetTime (Real fLength, int iIterations,
962  Real fTolerance) const
963 {
964  if (!m_afLength)
965  {
966  InitializeLength();
967  }
968 
969  if (fLength <= (Real)0.0)
970  {
971  return m_fTMin;
972  }
973 
974  if (fLength >= m_afAccumLength[m_iSegments-1])
975  {
976  return m_fTMax;
977  }
978 
979  int iKey;
980  for (iKey = 0; iKey < m_iSegments; iKey++)
981  {
982  if (fLength < m_afAccumLength[iKey])
983  {
984  break;
985  }
986  }
987  if (iKey >= m_iSegments)
988  {
989  return m_afTime[m_iSegments];
990  }
991 
992  // try Newton's method first for rapid convergence
993  Real fL0, fL1;
994  if (iKey == 0)
995  {
996  fL0 = fLength;
997  fL1 = m_afAccumLength[0];
998  }
999  else
1000  {
1001  fL0 = fLength - m_afAccumLength[iKey-1];
1002  fL1 = m_afAccumLength[iKey] - m_afAccumLength[iKey-1];
1003  }
1004 
1005  // use Newton's method to invert the arc length integral
1006  Real fDt1 = m_afTime[iKey+1] - m_afTime[iKey];
1007  Real fDt0 = fDt1*fL0/fL1;
1008  for (int i = 0; i < iIterations; i++)
1009  {
1010  Real fDifference = GetLengthKey(iKey,(Real)0.0,fDt0) - fL0;
1011  if (moMath<Real>::FAbs(fDifference) <= fTolerance)
1012  {
1013  return m_afTime[iKey] + fDt0;
1014  }
1015 
1016  fDt0 -= fDifference/GetSpeedKey(iKey,fDt0);
1017  }
1018 
1019  // Newton's method failed. You might want to increase iterations or
1020  // tolerance or integration accuracy. However, in this application it
1021  // is likely that the time values are oscillating, due to the limited
1022  // numerical precision of 32-bit floats. It is safe to use the last
1023  // computed time.
1024  return m_afTime[iKey] + fDt0;
1025 }
1026 */
1027 /*
1028 template <class Real>
1029 Real moMultipleCurve3<Real>::GetVariation (Real fT0, Real fT1,
1030  const moVector3<Real>* pkP0, const moVector3<Real>* pkP1) const
1031 {
1032  //assert(m_fTMin <= fT0 && fT0 <= m_fTMax);
1033  //assert(m_fTMin <= fT1 && fT1 <= m_fTMax);
1034  //assert(fT0 <= fT1);
1035 
1036  // construct line segment, A + (t-t0)*B
1037  moVector3<Real> kP0, kP1;
1038  if (!pkP0)
1039  {
1040  kP0 = GetPosition(fT0);
1041  pkP0 = &kP0;
1042  }
1043  if (!pkP1)
1044  {
1045  kP1 = GetPosition(fT1);
1046  pkP1 = &kP1;
1047  }
1048  Real fInvDT = ((Real)1.0)/(fT1 - fT0);
1049  moVector3<Real> kA, kB = fInvDT*(*pkP1 - *pkP0);
1050 
1051  int iKey0, iKey1;
1052  Real fDt0, fDt1;
1053  GetKeyInfo(fT0,iKey0,fDt0);
1054  GetKeyInfo(fT1,iKey1,fDt1);
1055 
1056  Real fVariation;
1057  if (iKey0 < iKey1)
1058  {
1059  // accumulate full-segment variations
1060  fVariation = (Real)0.0;
1061  for (int i = iKey0+1; i < iKey1; i++)
1062  {
1063  kA = *pkP0 + (m_afTime[i] - fT0)*kB;
1064  fVariation += GetVariationKey(i,(Real)0.0,
1065  m_afTime[i+1]-m_afTime[i],kA,kB);
1066  }
1067 
1068  // add on partial first segment
1069  kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
1070  fVariation += GetVariationKey(iKey0,fDt0,
1071  m_afTime[iKey0+1]-m_afTime[iKey0],kA,kB);
1072 
1073  // add on partial last segment
1074  kA = *pkP0 + (m_afTime[iKey1] - fT0)*kB;
1075  fVariation += GetVariationKey(iKey1,0.0f,fDt1,kA,kB);
1076  }
1077  else
1078  {
1079  kA = *pkP0 + (m_afTime[iKey0] - fT0)*kB;
1080  fVariation = GetVariationKey(iKey0,fDt0,fDt1,kA,kB);
1081  }
1082 
1083  return fVariation;
1084 }
1085 */
1086