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