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.
moMathCurve.cpp
Ir a la documentación de este archivo.
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