vector3.cpp

Go to the documentation of this file.
00001 /*
00002 This program is distributed under the terms of the 'MIT license'. The text
00003 of this licence follows...
00004 
00005 Copyright (c) 2005 J.D.Medhurst (a.k.a. Tixy)
00006 
00007 Permission is hereby granted, free of charge, to any person obtaining a copy
00008 of this software and associated documentation files (the "Software"), to deal
00009 in the Software without restriction, including without limitation the rights
00010 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011 copies of the Software, and to permit persons to whom the Software is
00012 furnished to do so, subject to the following conditions:
00013 
00014 The above copyright notice and this permission notice shall be included in
00015 all copies or substantial portions of the Software.
00016 
00017 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
00020 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023 THE SOFTWARE.
00024 */
00025 
00032 #include "common.h"
00033 #include "vector3.h"
00034 
00035 
00036 /*
00037 Members or class Vector3
00038 */
00039 
00040 
00041 Vector3 Vector3::operator - () const
00042     {
00043     return Vector3(-X,-Y,-Z);
00044     }
00045 
00046 
00047 Vector3 Vector3::operator + (const Vector3& vector) const
00048     {
00049     return Vector3(X+vector.X,Y+vector.Y,Z+vector.Z);
00050     }
00051 
00052 
00053 Vector3 Vector3::operator - (const Vector3& vector) const
00054     {
00055     return Vector3(X-vector.X,Y-vector.Y,Z-vector.Z);
00056     }
00057 
00058 
00059 Vector3 Vector3::operator * (fix scalar) const
00060     {
00061     return Vector3(Fix::MulNS(X,scalar),Fix::MulNS(Y,scalar),Fix::MulNS(Z,scalar));
00062     }
00063 
00064 
00065 Vector3 Vector3::operator / (fix scalar) const
00066     {
00067     return Vector3(Fix::Div(X,scalar),Fix::Div(Y,scalar),Fix::Div(Z,scalar));
00068     }
00069 
00070 
00071 Vector3& Vector3::operator += (const Vector3& vector)
00072     {
00073     X += vector.X;
00074     Y += vector.Y;
00075     Z += vector.Z;
00076     return *this;
00077     }
00078 
00079 
00080 Vector3& Vector3::operator -= (const Vector3& vector)
00081     {
00082     X -= vector.X;
00083     Y -= vector.Y;
00084     Z -= vector.Z;
00085     return *this;
00086     }
00087 
00088 
00089 Vector3& Vector3::operator *= (fix scalar)
00090     {
00091     X = Fix::MulNS(X,scalar);
00092     Y = Fix::MulNS(Y,scalar);
00093     Z = Fix::MulNS(Z,scalar);
00094     return *this;
00095     }
00096 
00097 
00098 Vector3& Vector3::operator /= (fix scalar)
00099     {
00100     X = Fix::Div(X,scalar);
00101     Y = Fix::Div(Y,scalar);
00102     Z = Fix::Div(Z,scalar);
00103     return *this;
00104     }
00105 
00106 
00107 bool Vector3::operator == (const Vector3& vector) const
00108     {
00109     return (X==vector.X && Y==vector.Y && Z==vector.Z);
00110     }
00111 
00112 
00113 fix Vector3::DotProduct(const Vector3& vector) const
00114     {
00115     return Fix::MulNS(X,vector.X)+Fix::MulNS(Y,vector.Y)+Fix::MulNS(Z,vector.Z);
00116     }
00117 
00118 
00119 Vector3 Vector3::CrossProduct(const Vector3& vector) const
00120     {
00121     return Vector3(Fix::MulNS(Y,vector.Z)-Fix::MulNS(Z,vector.Y),
00122                    Fix::MulNS(Z,vector.X)-Fix::MulNS(X,vector.Z),
00123                    Fix::MulNS(X,vector.Y)-Fix::MulNS(Y,vector.X));
00124     }
00125 
00126 
00127 static unsigned MostSignificantBit(uint32_t a)
00128     {
00129     unsigned b = 0;
00130     if(a>=(1<<16)) b |= 16, a >>= 16;
00131     if(a>=(1<<8))  b |= 8,  a >>= 8;
00132     if(a>=(1<<4))  b |= 4,  a >>= 4;
00133     if(a>=(1<<2))  b |= 2,  a >>= 2;
00134     b |= a>>1;
00135     return b;
00136     }
00137 
00138 
00139 ufix Vector3::Length() const
00140     {
00141     // calculate square of the length
00142     unsigned f;
00143     unsigned i = LengthSquared(f);
00144 
00145     // if no integer part, return the square root
00146     if(!i)
00147         return Fix::Sqrt(f)>>8;
00148 
00149     // shift square of result so it fits into 32 bits
00150     int shift = (2+MostSignificantBit(i))&~1;
00151     ufix s;
00152     if(shift==32)
00153         s = i;
00154     else
00155         {
00156         s = f>>shift;
00157         s |= i<<(32-shift);
00158         }
00159 
00160     // take the square root
00161     ufix r = Fix::Sqrt(s);
00162 
00163     // shift result to get binary point in the correct place
00164     shift = (16-shift)>>1;
00165     if(shift>0)
00166         r >>= shift;
00167     if(shift<0)
00168         r <<= -shift;
00169 
00170     return r;
00171     }
00172 
00173 
00174 int Vector3::CompareLength(ufix length) const
00175     {
00176     // calculate the square of this vectors length as ai:af
00177     uint32_t af;
00178     uint32_t ai = LengthSquared(af);
00179 
00180     // calculate the square of 'length' as bi:bf
00181     uint32_t l = length&0xFFFF;
00182     uint32_t h = length>>16;
00183     uint32_t bf = l*l;
00184     uint32_t bi = h*h;
00185     uint32_t r = l*h;
00186     uint32_t obf = bf;
00187     bf += r<<(16+1);
00188     if(bf<obf) bi++;
00189     bi += r>>(16-1);
00190 
00191     // compare the squares of the length
00192     if(ai<bi)
00193         return -1;
00194     if(ai>bi)
00195         return 1;
00196     if(af<bf)
00197         return -1;
00198     if(af>bf)
00199         return 1;
00200     return 0;
00201     }
00202 
00203 
00204 int Vector3::CompareLengths(const Vector3& vector) const
00205     {
00206     uint32_t af,bf;
00207     uint32_t ai = LengthSquared(af);
00208     uint32_t bi = vector.LengthSquared(bf);
00209 
00210     if(ai<bi)
00211         return -1;
00212     if(ai>bi)
00213         return 1;
00214     if(af<bf)
00215         return -1;
00216     if(af>bf)
00217         return 1;
00218     return 0;
00219     }
00220 
00221 
00222 uint32_t Vector3::LengthSquared(uint32_t& fraction) const
00223     {
00224     int32_t     v,r;
00225     uint32_t    l,h,lo,hi,ol;
00226 
00227     v = X;
00228     l = v&0xFFFF;
00229     h = v>>16;
00230     lo = l*l;
00231     hi = h*h;
00232     r = l*h;
00233     ol = lo;
00234     lo += r<<(16+1);
00235     if(lo<ol) hi++;
00236     hi += r>>(16-1);
00237 
00238     v = Y;
00239     l = v&0xFFFF;
00240     h = v>>16;
00241     ol = lo;
00242     lo += l*l;
00243     if(lo<ol) hi++;
00244     hi += h*h;
00245     r = l*h;
00246     ol = lo;
00247     lo += r<<(16+1);
00248     if(lo<ol) hi++;
00249     hi += r>>(16-1);
00250 
00251     v = Z;
00252     l = v&0xFFFF;
00253     h = v>>16;
00254     ol = lo;
00255     lo += l*l;
00256     if(lo<ol) hi++;
00257     hi += h*h;
00258     r = l*h;
00259     ol = lo;
00260     lo += r<<(16+1);
00261     if(lo<ol) hi++;
00262     hi += r>>(16-1);
00263 
00264     fraction = lo;
00265     return hi;
00266     }
00267 
00268 
00269 void Vector3::NormaliseComponents(unsigned bits)
00270     {
00271     // get components
00272     fix x = X;
00273     fix y = Y;
00274     fix z = Z;
00275 
00276     // find the magnitude of the largest component
00277     ufix max = x;
00278     if(x<0)
00279         max = ~x;
00280 
00281     ufix ay = y;
00282     if(y<0)
00283         ay = ~y;
00284     if(ay>max)
00285         max = ay;
00286 
00287     ufix az = z;
00288     if(z<0)
00289         az = ~z;
00290     if(az>max)
00291         max = az;
00292 
00293     // calculate shift value to get msb in correct place
00294     int shift = MostSignificantBit(max)-bits;
00295 
00296     // shift all components
00297     if(shift>0)
00298         {
00299         x >>= shift;
00300         y >>= shift;
00301         z >>= shift;
00302         }
00303     else if(shift<0)
00304         {
00305         shift = -shift;
00306         x <<= shift;
00307         y <<= shift;
00308         z <<= shift;
00309         }
00310 
00311     X = x;
00312     Y = y;
00313     Z = z;
00314     }
00315 
00316 
00317 Vector3 Vector3::UnitVector() const
00318     {
00319     // normalise component values to 15 bits
00320     Vector3 normalised(*this);
00321     normalised.NormaliseComponents(14);
00322 
00323     // calculate length of normalised vector
00324     fix x = normalised.X;
00325     fix y = normalised.Y;
00326     fix z = normalised.Z;
00327     ufix l = Fix::Sqrt(x*x+y*y+z*z);
00328 
00329     // divide components by length to get the unit vector
00330     x = Fix::Div(x<<8,l);
00331     y = Fix::Div(y<<8,l);
00332     z = Fix::Div(z<<8,l);
00333 
00334     return Vector3(x,y,z);
00335     }
00336 
00337 
00338 Vector3 Vector3::Normal(const Vector3& vector) const
00339     {
00340     // scale both vectors to 15 bits
00341     Vector3 a(*this);
00342     a.NormaliseComponents(14);
00343     Vector3 b(vector);
00344     b.NormaliseComponents(14);
00345 
00346     // calculate cross product (a vector normal to the two vectors)
00347     a = Vector3(a.Y*b.Z - a.Z*b.Y,
00348                 a.Z*b.X - a.X*b.Z,
00349                 a.X*b.Y - a.Y*b.X);
00350 
00351     // return the normal as a length 1.0 unit vector
00352     return a.UnitVector();
00353     }
00354 
00355 
00356 fixangle Vector3::Angle(const Vector3& vector) const
00357     {
00358     return Fix::ACos(UnitVector().DotProduct(vector.UnitVector()));
00359     }
00360 
00361 
00362 Vector3 Vector3::Normal(const Vector3& point1,const Vector3& point2) const
00363     {
00364     return (point1-*this).Normal(point2-*this);
00365     }
00366 
00367 
00368 fixangle Vector3::Angle(const Vector3& point1,const Vector3& point2) const
00369     {
00370     return (point1-*this).Angle(point2-*this);
00371     }
00372 
00373 
00374 void Vector3::Translate(Vector3* outVectors,unsigned vectorCount,const Vector3* inVectors,const Vector3& offset)
00375     {
00376     fix x = offset.X;
00377     fix y = offset.Y;
00378     fix z = offset.Z;
00379     Vector3* end = outVectors+vectorCount;
00380     while(outVectors<end)
00381         {
00382         outVectors->X = inVectors->X+x;
00383         outVectors->Y = inVectors->Y+y;
00384         outVectors->Z = inVectors->Z+z;
00385         ++inVectors;
00386         ++outVectors;
00387         }
00388     }
00389 
00390 
00391 void Vector3::Scale(Vector3* outVectors,unsigned vectorCount,const Vector3* inVectors,fix scale)
00392     {
00393     Vector3* end = outVectors+vectorCount;
00394     while(outVectors<end)
00395         {
00396         outVectors->X = Fix::MulNS(inVectors->X,scale);
00397         outVectors->Y = Fix::MulNS(inVectors->Y,scale);
00398         outVectors->Z = Fix::MulNS(inVectors->Z,scale);
00399         ++inVectors;
00400         ++outVectors;
00401         }
00402     }
00403 
00404 
00405 /*
00406 Members of class Matrix3
00407 */
00408 
00409 
00410 Vector3 Matrix3::operator * (const Vector3& vector) const
00411     {
00412     fix ax = vector.X;
00413     fix ay = vector.Y;
00414     fix az = vector.Z;
00415 
00416     fix rx = Fix::MulNS(ax,Row1.X)+Fix::MulNS(ay,Row1.Y)+Fix::MulNS(az,Row1.Z);
00417     fix ry = Fix::MulNS(ax,Row2.X)+Fix::MulNS(ay,Row2.Y)+Fix::MulNS(az,Row2.Z);
00418     fix rz = Fix::MulNS(ax,Row3.X)+Fix::MulNS(ay,Row3.Y)+Fix::MulNS(az,Row3.Z);
00419 
00420     return Vector3(rx,ry,rz);
00421     }
00422 
00423 
00424 Matrix3 Matrix3::Transposition() const
00425     {
00426     return Matrix3(Vector3(Row1.X,Row2.X,Row3.X),
00427                    Vector3(Row1.Y,Row2.Y,Row3.Y),
00428                    Vector3(Row1.Z,Row2.Z,Row3.Z));
00429     }
00430 
00431 
00432 void Matrix3::Transform(Vector3* outVectors,unsigned vectorCount,const Vector3* inVectors)
00433     {
00434     Vector3* end = outVectors+vectorCount;
00435     while(outVectors<end)
00436         *outVectors++ = *this * *inVectors++;
00437     }
00438 

Generated by  doxygen 1.6.1