The Cuik KD-Tree Library


vector.c

Go to the documentation of this file.
00001 #include "vector.h"
00002 
00003 #include "definitions.h"
00004 
00005 #include <assert.h>
00006 
00017 double VectorSquaredDistanceTopologyMin(double t2,unsigned int s,unsigned int *tp,
00018                                         double *v1,double *v2)
00019 {
00020   unsigned int i;
00021   double n,v;
00022   
00023   n=0.0;
00024   if (tp==NULL)
00025     {
00026       for(i=0;((i<s)&&(n<t2));i++)
00027         {
00028           v=v1[i]-v2[i];
00029           n+=(v*v);
00030         }
00031     }
00032   else
00033     {
00034       for(i=0;((i<s)&&(n<t2));i++)
00035         {
00036           v=v1[i]-v2[i];
00037           if ((tp[i]==TOPOLOGY_S)&&((v<-M_PI)||(v>M_PI)))
00038             PI2PI(v);
00039           n+=(v*v);
00040         }
00041     }
00042   return(n);
00043 }
00044 
00045 double VectorNorm(unsigned int s,double *v)
00046 {
00047   unsigned int i;
00048   double n;
00049 
00050   n=0.0;
00051   for(i=0;i<s;i++)
00052     n+=(v[i]*v[i]);
00053   n=sqrt(n);
00054 
00055   return(n);
00056 }
00057 
00058 void VectorNormalize(unsigned int s,double *v)
00059 {
00060   unsigned int i;
00061   double n;
00062   
00063   n=VectorNorm(s,v);
00064 
00065   assert(n>1e-6);
00066 
00067   for(i=0;i<s;i++)
00068     v[i]/=n;
00069 }