00001 #include "path.h"
00002
00007 #include <cmath>
00008 #include <cassert>
00009 using namespace std;
00010
00011 GltPath3D::GltPath3D()
00012 {
00013 }
00014
00015 GltPath3D::~GltPath3D()
00016 {
00017 }
00018
00020
00021 GltPath3DLine::GltPath3DLine(const Vector &origin,const Vector &direction)
00022 : _origin(origin), _direction(direction)
00023 {
00024 }
00025
00026 Vector
00027 GltPath3DLine::f(const real t) const
00028 {
00029 return _origin + _direction*t;
00030 }
00031
00032 Vector
00033 GltPath3DLine::df(const real t) const
00034 {
00035 return _direction;
00036 }
00037
00038 Vector
00039 GltPath3DLine::ddf(const real t) const
00040 {
00041 return Vector0;
00042 }
00043
00045
00046 GltPath3DHelix::GltPath3DHelix(const Vector &origin,const Vector &direction,const real radius,const real freq,const real phase)
00047 : _origin(origin), _direction(direction), _radius(radius), _freq(freq), _phase(phase)
00048 {
00049 Vector dir = _direction;
00050 dir.normalize();
00051 const real angle = acos(dir*VectorZ);
00052
00053 if (fabs(angle)>0.01)
00054 _trans = matrixRotate(xProduct(_direction,VectorZ),angle);
00055 }
00056
00057 Vector
00058 GltPath3DHelix::f(const real t) const
00059 {
00060 Vector orbit = VectorX*sin(t*_freq*M_2PI+_phase) + VectorY*cos(t*_freq*M_2PI+_phase);
00061 orbit = orbit * _radius;
00062 return _origin + _direction*t + _trans*orbit;
00063 }
00064
00065 Vector
00066 GltPath3DHelix::df(const real t) const
00067 {
00068 Vector orbit = VectorX*cos(t*_freq*M_2PI+_phase) - VectorY*sin(t*_freq*M_2PI+_phase);
00069 orbit = orbit * (_radius * _freq * M_2PI);
00070
00071 return _direction + _trans*orbit;
00072 }
00073
00074 Vector
00075 GltPath3DHelix::ddf(const real t) const
00076 {
00077 Vector orbit = -VectorX*sin(t*_freq*M_2PI+_phase) - VectorY*cos(t*_freq*M_2PI+_phase);
00078 orbit = orbit * _radius * SQ(_freq*M_2PI);
00079
00080 return _trans*orbit;
00081 }
00082
00084
00085 GltPath3DSegment::GltPath3DSegment(const GltPath3D &func,const real begin,const real end)
00086 : _func(func), _begin(begin), _end(end)
00087 {
00088 assert(_begin<_end);
00089 assert(CLAMP<real>(_begin,0.0,1.0)==_begin);
00090 assert(CLAMP<real>(_end,0.0,1.0)==_end);
00091 }
00092
00093
00094 Vector
00095 GltPath3DSegment::f(const real t) const
00096 {
00097 return _func.f(segT(t));
00098 }
00099
00100 Vector
00101 GltPath3DSegment::df(const real t) const
00102 {
00103 return _func.df(segT(t));
00104 }
00105
00106 Vector
00107 GltPath3DSegment::ddf(const real t) const
00108 {
00109 return _func.ddf(segT(t));
00110 }
00111
00112 real
00113 GltPath3DSegment::segT(const real t) const
00114 {
00115 return (_end-_begin)*t+_begin;
00116 }
00117
00118
00119
