00001
00002 #include "Point.hpp"
00003
00004 #include <sstream>
00005
00006 namespace jafar {
00007 namespace geom {
00008
00009 template<int dimension>
00010 struct Segment<dimension>::LineDriver::Private {
00011 const Line<dimension>* support;
00012 double firstIndex, firstIndexCov;
00013 double lastIndex, lastIndexCov;
00014 };
00015 template<int dimension>
00016 Segment<dimension>::LineDriver::LineDriver(const Line<dimension>* support, double firstIndex, double lastIndex, const RepereD* reference) : Line<dimension>::Driver( ), d(new Private)
00017 {
00018 d->support = support;
00019 d->firstIndex = firstIndex;
00020 d->firstIndexCov = 0.0;
00021 d->lastIndex = lastIndex;
00022 d->lastIndexCov = 0.0;
00023 }
00024 template<int dimension>
00025 Segment<dimension>::LineDriver::~LineDriver()
00026 {
00027 delete d;
00028 }
00029 template<int dimension>
00030 Line<dimension> Segment<dimension>::LineDriver::support() const
00031 {
00032 return *d->support;
00033 }
00034 template<int dimension>
00035 typename Segment<dimension>::HomogenousVecD Segment<dimension>::LineDriver::firstPoint() const
00036 {
00037 return d->support->pointAt(d->firstIndex);
00038 }
00039 template<int dimension>
00040 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::LineDriver::firstPointCov() const
00041 {
00042 return d->support->pointAtCov(d->firstIndex, d->firstIndexCov);
00043 }
00044 template<int dimension>
00045 typename Segment<dimension>::HomogenousVecD Segment<dimension>::LineDriver::lastPoint() const
00046 {
00047 return d->support->pointAt(d->lastIndex);
00048 }
00049 template<int dimension>
00050 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::LineDriver::lastPointCov() const
00051 {
00052 return d->support->pointAtCov(d->lastIndex, d->lastIndexCov);
00053 }
00054 template<int dimension>
00055 typename Segment<dimension>::HomogenousVecD Segment<dimension>::LineDriver::pointAt(double t) const
00056 {
00057 return d->support->pointAt(t * (d->lastIndex - d->firstIndex) + d->firstIndex);
00058 }
00059 template<int dimension>
00060 typename Segment<dimension>::HomogenousVecD Segment<dimension>::LineDriver::origin() const
00061 {
00062 return d->support->origin();
00063 }
00064 template<int dimension>
00065 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::LineDriver::originCov() const
00066 {
00067 return d->support->originCov();
00068 }
00069 template<int dimension>
00070 typename Segment<dimension>::HomogenousVecD Segment<dimension>::LineDriver::direction() const
00071 {
00072 return d->support->direction();
00073 }
00074 template<int dimension>
00075 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::LineDriver::directionCov() const
00076 {
00077 return d->support->directionCov();
00078 }
00079 template<int dimension>
00080 typename Segment<dimension>::Driver* Segment<dimension>::LineDriver::cloneSegmentDriver() const
00081 {
00082 return new LineDriver( d->support, d->firstIndex, d->lastIndex );
00083 }
00084 template<int dimension>
00085 typename Line<dimension>::Driver* Segment<dimension>::LineDriver::cloneLineDriver() const
00086 {
00087 return d->support->clonedDriver();
00088 }
00089 template<int dimension>
00090 typename Line<dimension>::Driver* Segment<dimension>::LineDriver::asLineDriver()
00091 {
00092 return this;
00093 }
00094 template<int dimension>
00095 const Repere<dimension>* Segment<dimension>::LineDriver::reference( ) const
00096 {
00097 return d->support->reference();
00098 }
00099 template<int dimension>
00100 bool Segment<dimension>::LineDriver::hasCov() const
00101 {
00102 return d->support->hasCov();
00103 }
00104
00105 template<int dimension>
00106 struct Segment<dimension>::TwoPointsPointerDriver::Private {
00107 const Point<dimension>* firstPoint;
00108 const Point<dimension>* lastPoint;
00109 };
00110 template<int dimension>
00111 Segment<dimension>::TwoPointsPointerDriver::TwoPointsPointerDriver(const Point<dimension>* p1, const Point<dimension>* p2) : Line<dimension>::TwoPointsPointerDriver(p1, p2), d(new Private)
00112 {
00113 d->firstPoint = p1;
00114 d->lastPoint = p2;
00115 }
00116 template<int dimension>
00117 Segment<dimension>::TwoPointsPointerDriver::~TwoPointsPointerDriver()
00118 {
00119 delete d;
00120 }
00121 template<int dimension>
00122 Line<dimension> Segment<dimension>::TwoPointsPointerDriver::support() const
00123 {
00124 return Line<dimension>( new typename Line<dimension>::TwoPointsPointerDriver(d->firstPoint, d->lastPoint ) );
00125 }
00126 template<int dimension>
00127 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsPointerDriver::firstPoint() const
00128 {
00129 return d->firstPoint->homogenousCoordinates();
00130 }
00131 template<int dimension>
00132 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsPointerDriver::firstPointCov() const
00133 {
00134 return d->firstPoint->homogenousCoordinatesCov();
00135 }
00136 template<int dimension>
00137 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsPointerDriver::lastPoint() const
00138 {
00139 return d->lastPoint->reference()->convertIn(d->lastPoint->homogenousCoordinates(), d->firstPoint->reference());
00140 }
00141 template<int dimension>
00142 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsPointerDriver::lastPointCov() const
00143 {
00144 return d->lastPoint->homogenousCoordinatesCov();
00145 }
00146 template<int dimension>
00147 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsPointerDriver::pointAt(double t) const
00148 {
00149 return (1.0 - t) * firstPoint() + t * lastPoint();
00150 }
00151 template<int dimension>
00152 typename Segment<dimension>::Driver* Segment<dimension>::TwoPointsPointerDriver::cloneSegmentDriver() const
00153 {
00154 return new TwoPointsPointerDriver( d->firstPoint, d->lastPoint);
00155 }
00156 template<int dimension>
00157 typename Line<dimension>::Driver* Segment<dimension>::TwoPointsPointerDriver::asLineDriver()
00158 {
00159 return this;
00160 }
00161 template<int dimension>
00162 const Repere<dimension>* Segment<dimension>::TwoPointsPointerDriver::reference( ) const
00163 {
00164 return d->firstPoint->reference();
00165 }
00166 template<int dimension>
00167 bool Segment<dimension>::TwoPointsPointerDriver::hasCov() const
00168 {
00169 return d->firstPoint->hasCov() and d->lastPoint->hasCov();
00170 }
00171
00172 template<int dimension>
00173 Segment<dimension>::TwoPointsDriver::TwoPointsDriver(const HomogenousVecD& p1, const HomogenousVecD& p2, const RepereD* reference) : Line<dimension>::Driver(), m_firstPoint(p1), m_lastPoint(p2), m_reference(reference), m_hasCov(false)
00174 {
00175 }
00176 template<int dimension>
00177 Segment<dimension>::TwoPointsDriver::~TwoPointsDriver()
00178 {
00179 }
00180 template<int dimension>
00181 void Segment<dimension>::TwoPointsDriver::setCov(HomogenousSymMatrixD _firstPointCov, HomogenousSymMatrixD _lastPointCov)
00182 {
00183 m_firstPointCov = _firstPointCov;
00184 m_lastPointCov = _lastPointCov;
00185 m_hasCov = true;
00186 }
00187 template<int dimension>
00188 Line<dimension> Segment<dimension>::TwoPointsDriver::support() const
00189 {
00190 return Line<dimension>( cloneLineDriver() );
00191 }
00192 template<int dimension>
00193 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsDriver::firstPoint() const
00194 {
00195 return m_firstPoint;
00196 }
00197 template<int dimension>
00198 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsDriver::firstPointCov() const
00199 {
00200 return m_firstPointCov;
00201 }
00202 template<int dimension>
00203 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsDriver::lastPoint() const
00204 {
00205 return m_lastPoint;
00206 }
00207 template<int dimension>
00208 typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsDriver::lastPointCov() const
00209 {
00210 return m_lastPointCov;
00211 }
00212 template<int dimension>
00213 typename Segment<dimension>::HomogenousVecD Segment<dimension>::TwoPointsDriver::pointAt(double t) const
00214 {
00215 return (1.0 - t) * m_firstPoint + t * m_lastPoint;
00216 }
00217 template<int dimension>
00218 typename Segment<dimension>::Driver* Segment<dimension>::TwoPointsDriver::cloneSegmentDriver() const
00219 {
00220 return new TwoPointsDriver( *this );
00221 }
00222 template<int dimension>
00223 void Segment<dimension>::TwoPointsDriver::changeReference( const RepereD* reference )
00224 {
00225 HomogenousMatrixD m;
00226 reference->repereToLocal(m, this->m_reference);
00227 m_firstPoint = ublas::prod(m, m_firstPoint);
00228 m_lastPoint = ublas::prod(m, m_lastPoint);
00229 this->m_reference = reference;
00230 }
00231 template<int dimension>
00232 const Repere<dimension>* Segment<dimension>::TwoPointsDriver::reference( ) const
00233 {
00234 return m_reference;
00235 }
00236 template<int dimension>
00237 typename Line<dimension>::Driver* Segment<dimension>::TwoPointsDriver::asLineDriver()
00238 {
00239 return this;
00240 }
00241 template<int dimension>
00242 typename Atom<dimension>::HomogenousVecD Segment<dimension>::TwoPointsDriver::origin() const
00243 {
00244 return m_firstPoint;
00245 }
00246 template<int dimension>
00247 typename Atom<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsDriver::originCov() const
00248 {
00249 return m_firstPointCov;
00250 }
00251 template<int dimension>
00252 typename Atom<dimension>::HomogenousVecD Segment<dimension>::TwoPointsDriver::direction() const
00253 {
00254 HomogenousVecD v = m_lastPoint - m_firstPoint;
00255 return v / ublas::norm_2(v);
00256 }
00257 template<int dimension>
00258 typename Atom<dimension>::HomogenousSymMatrixD Segment<dimension>::TwoPointsDriver::directionCov() const
00259 {
00260 HomogenousVecD v = m_lastPoint - m_firstPoint;
00261 return (m_firstPointCov + m_lastPointCov) / ublas::inner_prod(v,v);
00262 }
00263 template<int dimension>
00264 typename Line<dimension>::Driver* Segment<dimension>::TwoPointsDriver::cloneLineDriver() const
00265 {
00266 typename Line<dimension>::EuclideanDriver* edriver = new typename Line<dimension>::EuclideanDriver( origin(), direction(), this->reference() );
00267 if(hasCov())
00268 {
00269 edriver->setCov( originCov(), directionCov());
00270 }
00271 return edriver;
00272 }
00273 template<int dimension>
00274 void Segment<dimension>::TwoPointsDriver::applyTransformation( const HomogenousMatrixD& _transformation )
00275 {
00276 m_firstPoint = ublas::prod( _transformation, m_firstPoint );
00277 m_lastPoint = ublas::prod( _transformation, m_lastPoint );
00278 JFR_ASSERT( not hasCov(), "Transformation of covariance not supported" );
00279 }
00280
00281 template<int dimension>
00282 Segment<dimension>::Segment() : Line<dimension>(0), m_driver(0)
00283 {
00284
00285 }
00286 template<int dimension>
00287 template<class _TDriver_>
00288 Segment<dimension>::Segment(_TDriver_* driver) : Line<dimension>(driver), m_driver(driver)
00289 {
00290 }
00291 template<int dimension>
00292 Segment<dimension>::Segment(const Segment& s) : Line<dimension>(0), m_driver( 0 )
00293 {
00294 *this = s;
00295 }
00296 template<int dimension>
00297 Segment<dimension>& Segment<dimension>::operator=(const Segment<dimension>& s)
00298 {
00299 if(s.m_driver)
00300 {
00301 m_driver = s.m_driver->cloneSegmentDriver();
00302 JFR_ASSERT( m_driver->asLineDriver()->hasCov() == s.m_driver->asLineDriver()->hasCov(), "bug" );
00303 this-> setDriver( m_driver->asLineDriver() );
00304 } else {
00305 m_driver = 0;
00306 this->setDriver(0);
00307 }
00308 this->setId( s.id() );
00309 return *this;
00310 }
00311 template<int dimension>
00312 Segment<dimension>::~Segment()
00313 {
00314 }
00315 template<int dimension>
00316 BoundingBox<dimension> Segment<dimension>::boundingBox() const
00317 {
00318 return BoundingBox<dimension>(firstPoint(), lastPoint());
00319 }
00320 template<int dimension>
00321 inline Line<dimension> Segment<dimension>::support() const
00322 {
00323 return m_driver->support();
00324 }
00325 template<int dimension>
00326 inline typename Segment<dimension>::HomogenousVecD Segment<dimension>::firstPoint() const
00327 {
00328 return m_driver->firstPoint();
00329 }
00330 template<int dimension>
00331 inline typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::firstPointCov() const
00332 {
00333 return m_driver->firstPointCov();
00334 }
00335 template<int dimension>
00336 inline typename Segment<dimension>::HomogenousVecD Segment<dimension>::lastPoint() const
00337 {
00338 return m_driver->lastPoint();
00339 }
00340 template<int dimension>
00341 inline typename Segment<dimension>::HomogenousSymMatrixD Segment<dimension>::lastPointCov() const
00342 {
00343 return m_driver->lastPointCov();
00344 }
00345 template<int dimension>
00346 inline double Segment<dimension>::length() const
00347 {
00348 return ublas::norm_2( firstPoint() - lastPoint() );
00349 }
00350 template<int dimension>
00351 std::string Segment<dimension>::toString() const
00352 {
00353 std::ostringstream oss;
00354 oss << Atom<dimension>::toString() << " FirstPoint = " << firstPoint() << " LastPoint = " << lastPoint() << " Length = " << length();
00355 return oss.str();
00356 }
00357 }
00358 }
