Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #ifndef __VECTORIMPL_HPP 00030 #define __VECTORIMPL_HPP 00031 00032 //////////// 00033 // Vector // 00034 //////////// 00035 namespace pcl 00036 { 00037 namespace poisson 00038 { 00039 00040 00041 template<class T> 00042 Vector<T>::Vector() 00043 { 00044 m_N = 0; 00045 m_pV = 0; 00046 } 00047 template<class T> 00048 Vector<T>::Vector( const Vector<T>& V ) 00049 { 00050 m_N = 0; 00051 m_pV = 0; 00052 Resize(V.m_N); 00053 memcpy( m_pV, V.m_pV, m_N*sizeof(T) ); 00054 } 00055 template<class T> 00056 Vector<T>::Vector( size_t N ) 00057 { 00058 m_N=0; 00059 m_pV=0; 00060 Resize(N); 00061 } 00062 template<class T> 00063 void Vector<T>::Resize( size_t N ) 00064 { 00065 if(m_N!=N){ 00066 if(m_N){delete[] m_pV;} 00067 m_pV=NULL; 00068 m_N = N; 00069 if(N){m_pV = new T[N];} 00070 } 00071 memset( m_pV, 0, N*sizeof(T) ); 00072 } 00073 template<class T> 00074 Vector<T>::Vector( size_t N, T* pV ) 00075 { 00076 Resize(N); 00077 memcpy( m_pV, pV, N*sizeof(T) ); 00078 } 00079 template<class T> 00080 Vector<T>::~Vector(){Resize(0);} 00081 template<class T> 00082 Vector<T>& Vector<T>::operator = (const Vector& V) 00083 { 00084 Resize(V.m_N); 00085 memcpy( m_pV, V.m_pV, m_N*sizeof(T) ); 00086 return *this; 00087 } 00088 template<class T> 00089 size_t Vector<T>::Dimensions() const{return m_N;} 00090 template<class T> 00091 void Vector<T>::SetZero(void){for (size_t i=0; i<m_N; i++){m_pV[i] = T(0);}} 00092 template<class T> 00093 const T& Vector<T>::operator () (size_t i) const 00094 { 00095 Assert( i < m_N ); 00096 return m_pV[i]; 00097 } 00098 template<class T> 00099 T& Vector<T>::operator () (size_t i) 00100 { 00101 return m_pV[i]; 00102 } 00103 template<class T> 00104 const T& Vector<T>::operator [] (size_t i) const 00105 { 00106 return m_pV[i]; 00107 } 00108 template<class T> 00109 T& Vector<T>::operator [] (size_t i) 00110 { 00111 return m_pV[i]; 00112 } 00113 template<class T> 00114 Vector<T> Vector<T>::operator * (const T& A) const 00115 { 00116 Vector V(*this); 00117 for (size_t i=0; i<m_N; i++) 00118 V.m_pV[i] *= A; 00119 return V; 00120 } 00121 template<class T> 00122 Vector<T>& Vector<T>::operator *= (const T& A) 00123 { 00124 for (size_t i=0; i<m_N; i++) 00125 m_pV[i] *= A; 00126 return *this; 00127 } 00128 template<class T> 00129 Vector<T> Vector<T>::operator / (const T& A) const 00130 { 00131 Vector V(*this); 00132 for (size_t i=0; i<m_N; i++) 00133 V.m_pV[i] /= A; 00134 return V; 00135 } 00136 template<class T> 00137 Vector<T>& Vector<T>::operator /= (const T& A) 00138 { 00139 for (size_t i=0; i<m_N; i++) 00140 m_pV[i] /= A; 00141 return *this; 00142 } 00143 template<class T> 00144 Vector<T> Vector<T>::operator + (const Vector<T>& V0) const 00145 { 00146 Vector<T> V(m_N); 00147 for (size_t i=0; i<m_N; i++) 00148 V.m_pV[i] = m_pV[i] + V0.m_pV[i]; 00149 00150 return V; 00151 } 00152 template<class T> 00153 Vector<T>& Vector<T>::AddScaled(const Vector<T>& V,const T& scale) 00154 { 00155 for (size_t i=0; i<m_N; i++) 00156 m_pV[i] += V.m_pV[i]*scale; 00157 00158 return *this; 00159 } 00160 template<class T> 00161 Vector<T>& Vector<T>::SubtractScaled(const Vector<T>& V,const T& scale) 00162 { 00163 for (size_t i=0; i<m_N; i++) 00164 m_pV[i] -= V.m_pV[i]*scale; 00165 00166 return *this; 00167 } 00168 template<class T> 00169 void Vector<T>::Add(const Vector<T>& V1,const T& scale1,const Vector<T>& V2,const T& scale2,Vector<T>& Out){ 00170 for (size_t i=0; i<V1.m_N; i++) 00171 Out.m_pV[i]=V1.m_pV[i]*scale1+V2.m_pV[i]*scale2; 00172 } 00173 template<class T> 00174 void Vector<T>::Add(const Vector<T>& V1,const T& scale1,const Vector<T>& V2,Vector<T>& Out){ 00175 for (size_t i=0; i<V1.m_N; i++) 00176 Out.m_pV[i]=V1.m_pV[i]*scale1+V2.m_pV[i]; 00177 } 00178 template<class T> 00179 Vector<T>& Vector<T>::operator += (const Vector<T>& V) 00180 { 00181 for (size_t i=0; i<m_N; i++) 00182 m_pV[i] += V.m_pV[i]; 00183 00184 return *this; 00185 } 00186 template<class T> 00187 Vector<T> Vector<T>::operator - (const Vector<T>& V0) const 00188 { 00189 Vector<T> V(m_N); 00190 for (size_t i=0; i<m_N; i++) 00191 V.m_pV[i] = m_pV[i] - V0.m_pV[i]; 00192 00193 return V; 00194 } 00195 template<class T> 00196 Vector<T> Vector<T>::operator - (void) const 00197 { 00198 Vector<T> V(m_N); 00199 00200 for (size_t i=0; i<m_N; i++) 00201 V.m_pV[i] = -m_pV[i]; 00202 00203 return V; 00204 } 00205 template<class T> 00206 Vector<T>& Vector<T>::operator -= (const Vector<T>& V) 00207 { 00208 for (size_t i=0; i<m_N; i++) 00209 m_pV[i] -= V.m_pV[i]; 00210 00211 return *this; 00212 } 00213 template<class T> 00214 T Vector<T>::Norm( size_t Ln ) const 00215 { 00216 T N = T(); 00217 for (size_t i = 0; i<m_N; i++) 00218 N += pow(m_pV[i], (T)Ln); 00219 return pow(N, (T)1.0/Ln); 00220 } 00221 template<class T> 00222 void Vector<T>::Normalize() 00223 { 00224 T N = 1.0f/Norm(2); 00225 for (size_t i = 0; i<m_N; i++) 00226 m_pV[i] *= N; 00227 } 00228 template<class T> 00229 T Vector<T>::Length() const 00230 { 00231 T N = T(); 00232 for (size_t i = 0; i<m_N; i++) 00233 N += m_pV[i]*m_pV[i]; 00234 return sqrt(N); 00235 } 00236 template<class T> 00237 T Vector<T>::Dot( const Vector<T>& V ) const 00238 { 00239 T V0 = T(); 00240 for (size_t i=0; i<m_N; i++) 00241 V0 += m_pV[i]*V.m_pV[i]; 00242 00243 return V0; 00244 } 00245 00246 template< class T > 00247 bool Vector< T >::read( const char* fileName ) 00248 { 00249 FILE* fp = fopen( fileName , "rb" ); 00250 if( !fp ) return false; 00251 bool ret = read( fp ); 00252 fclose( fp ); 00253 return ret; 00254 } 00255 template< class T > 00256 bool Vector< T >::write( const char* fileName ) const 00257 { 00258 FILE* fp = fopen( fileName , "wb" ); 00259 if( !fp ) return false; 00260 bool ret = write( fp ); 00261 fclose( fp ); 00262 return ret; 00263 } 00264 template< class T > 00265 bool Vector< T >::read( FILE* fp ) 00266 { 00267 int d; 00268 if( fread( &d , sizeof(int) , 1 , fp )!=1 ) return false; 00269 Resize( d ); 00270 if( fread( &(*this)[0] , sizeof( T ) , d , fp )!=d ) return false; 00271 return true; 00272 } 00273 template< class T > 00274 bool Vector< T >::write( FILE* fp ) const 00275 { 00276 if( fwrite( &m_N , sizeof( int ) , 1 , fp )!=1 ) return false; 00277 if( fwrite( &(*this)[0] , sizeof( T ) , m_N , fp )!=m_N ) return false; 00278 return true; 00279 } 00280 00281 00282 ///////////// 00283 // NVector // 00284 ///////////// 00285 template<class T,int Dim> 00286 NVector<T,Dim>::NVector() 00287 { 00288 m_N = 0; 00289 m_pV = 0; 00290 } 00291 template<class T,int Dim> 00292 NVector<T,Dim>::NVector( const NVector<T,Dim>& V ) 00293 { 00294 m_N = 0; 00295 m_pV = 0; 00296 Resize(V.m_N); 00297 memcpy( m_pV, V.m_pV, m_N*sizeof(T)*Dim ); 00298 } 00299 template<class T,int Dim> 00300 NVector<T,Dim>::NVector( size_t N ) 00301 { 00302 m_N=0; 00303 m_pV=0; 00304 Resize(N); 00305 } 00306 template<class T,int Dim> 00307 void NVector<T,Dim>::Resize( size_t N ) 00308 { 00309 if(m_N!=N){ 00310 if(m_N){delete[] m_pV;} 00311 m_pV=NULL; 00312 m_N = N; 00313 if(N){m_pV = new T[Dim*N];} 00314 } 00315 memset( m_pV, 0, N*sizeof(T)*Dim ); 00316 } 00317 template<class T,int Dim> 00318 NVector<T,Dim>::NVector( size_t N, T* pV ) 00319 { 00320 Resize(N); 00321 memcpy( m_pV, pV, N*sizeof(T)*Dim ); 00322 } 00323 template<class T,int Dim> 00324 NVector<T,Dim>::~NVector(){Resize(0);} 00325 template<class T,int Dim> 00326 NVector<T,Dim>& NVector<T,Dim>::operator = (const NVector& V) 00327 { 00328 Resize(V.m_N); 00329 memcpy( m_pV, V.m_pV, m_N*sizeof(T)*Dim ); 00330 return *this; 00331 } 00332 template<class T,int Dim> 00333 size_t NVector<T,Dim>::Dimensions() const{return m_N;} 00334 template<class T,int Dim> 00335 void NVector<T,Dim>::SetZero(void){for (size_t i=0; i<m_N*Dim; i++){m_pV[i] = T(0);}} 00336 template<class T,int Dim> 00337 const T* NVector<T,Dim>::operator () (size_t i) const 00338 { 00339 Assert( i < m_N ); 00340 return &m_pV[i*Dim]; 00341 } 00342 template<class T,int Dim> 00343 T* NVector<T,Dim>::operator () (size_t i) 00344 { 00345 return &m_pV[i*Dim]; 00346 } 00347 template<class T,int Dim> 00348 const T* NVector<T,Dim>::operator [] (size_t i) const 00349 { 00350 return &m_pV[i*Dim]; 00351 } 00352 template<class T,int Dim> 00353 T* NVector<T,Dim>::operator [] (size_t i) 00354 { 00355 return &m_pV[i*Dim]; 00356 } 00357 template<class T,int Dim> 00358 NVector<T,Dim> NVector<T,Dim>::operator * (const T& A) const 00359 { 00360 NVector<T,Dim> V(*this); 00361 for (size_t i=0; i<m_N*Dim; i++) 00362 V.m_pV[i] *= A; 00363 return V; 00364 } 00365 template<class T,int Dim> 00366 NVector<T,Dim>& NVector<T,Dim>::operator *= (const T& A) 00367 { 00368 for (size_t i=0; i<m_N*Dim; i++) 00369 m_pV[i] *= A; 00370 return *this; 00371 } 00372 template<class T,int Dim> 00373 NVector<T,Dim> NVector<T,Dim>::operator / (const T& A) const 00374 { 00375 NVector<T,Dim> V(*this); 00376 for (size_t i=0; i<m_N*Dim; i++) 00377 V.m_pV[i] /= A; 00378 return V; 00379 } 00380 template<class T,int Dim> 00381 NVector<T,Dim>& NVector<T,Dim>::operator /= (const T& A) 00382 { 00383 for (size_t i=0; i<m_N*Dim; i++) 00384 m_pV[i] /= A; 00385 return *this; 00386 } 00387 template<class T,int Dim> 00388 NVector<T,Dim> NVector<T,Dim>::operator + (const NVector<T,Dim>& V0) const 00389 { 00390 NVector<T,Dim> V(m_N); 00391 for (size_t i=0; i<m_N*Dim; i++) 00392 V.m_pV[i] = m_pV[i] + V0.m_pV[i]; 00393 00394 return V; 00395 } 00396 template<class T,int Dim> 00397 NVector<T,Dim>& NVector<T,Dim>::AddScaled(const NVector<T,Dim>& V,const T& scale) 00398 { 00399 for (size_t i=0; i<m_N*Dim; i++) 00400 m_pV[i] += V.m_pV[i]*scale; 00401 00402 return *this; 00403 } 00404 template<class T,int Dim> 00405 NVector<T,Dim>& NVector<T,Dim>::SubtractScaled(const NVector<T,Dim>& V,const T& scale) 00406 { 00407 for (size_t i=0; i<m_N*Dim; i++) 00408 m_pV[i] -= V.m_pV[i]*scale; 00409 00410 return *this; 00411 } 00412 template<class T,int Dim> 00413 void NVector<T,Dim>::Add(const NVector<T,Dim>& V1,const T& scale1,const NVector<T,Dim>& V2,const T& scale2,NVector<T,Dim>& Out){ 00414 for (size_t i=0; i<V1.m_N*Dim; i++) 00415 Out.m_pV[i]=V1.m_pV[i]*scale1+V2.m_pV[i]*scale2; 00416 } 00417 template<class T,int Dim> 00418 void NVector<T,Dim>::Add(const NVector<T,Dim>& V1,const T& scale1,const NVector<T,Dim>& V2,NVector<T,Dim>& Out){ 00419 for (size_t i=0; i<V1.m_N*Dim; i++) 00420 Out.m_pV[i]=V1.m_pV[i]*scale1+V2.m_pV[i]; 00421 } 00422 template<class T,int Dim> 00423 NVector<T,Dim>& NVector<T,Dim>::operator += (const NVector<T,Dim>& V) 00424 { 00425 for (size_t i=0; i<m_N*Dim; i++) 00426 m_pV[i] += V.m_pV[i]; 00427 00428 return *this; 00429 } 00430 template<class T,int Dim> 00431 NVector<T,Dim> NVector<T,Dim>::operator - (const NVector<T,Dim>& V0) const 00432 { 00433 NVector<T,Dim> V(m_N); 00434 for (size_t i=0; i<m_N*Dim; i++) 00435 V.m_pV[i] = m_pV[i] - V0.m_pV[i]; 00436 00437 return V; 00438 } 00439 template<class T,int Dim> 00440 NVector<T,Dim> NVector<T,Dim>::operator - (void) const 00441 { 00442 NVector<T,Dim> V(m_N); 00443 00444 for (size_t i=0; i<m_N*Dim; i++) 00445 V.m_pV[i] = -m_pV[i]; 00446 00447 return V; 00448 } 00449 template<class T,int Dim> 00450 NVector<T,Dim>& NVector<T,Dim>::operator -= (const NVector<T,Dim>& V) 00451 { 00452 for (size_t i=0; i<m_N*Dim; i++) 00453 m_pV[i] -= V.m_pV[i]; 00454 00455 return *this; 00456 } 00457 template<class T,int Dim> 00458 T NVector<T,Dim>::Norm( size_t Ln ) const 00459 { 00460 T N = T(); 00461 for (size_t i = 0; i<m_N*Dim; i++) 00462 N += pow(m_pV[i], (T)Ln); 00463 return pow(N, (T)1.0/Ln); 00464 } 00465 template<class T,int Dim> 00466 void NVector<T,Dim>::Normalize() 00467 { 00468 T N = 1.0f/Norm(2); 00469 for (size_t i = 0; i<m_N*3; i++) 00470 m_pV[i] *= N; 00471 } 00472 template<class T,int Dim> 00473 T NVector<T,Dim>::Length() const 00474 { 00475 T N = T(); 00476 for (size_t i = 0; i<m_N*Dim; i++) 00477 N += m_pV[i]*m_pV[i]; 00478 return sqrt(N); 00479 } 00480 template<class T,int Dim> 00481 T NVector<T,Dim>::Dot( const NVector<T,Dim>& V ) const 00482 { 00483 T V0 = T(); 00484 for (size_t i=0; i<m_N*Dim; i++) 00485 V0 += m_pV[i]*V.m_pV[i]; 00486 00487 return V0; 00488 } 00489 00490 } 00491 } 00492 #endif