vector3.cpp

Tixys source code, include G.711, G.726, IMA-ADPCM etc.

/**
@file

@brief 3D vector maths using fixed-point arithmatic

For latest source code see http://www.tixy.clara.net/source/

Copyright (C) 2005 J.D.Medhurst (a.k.a. Tixy)

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "common.h"
#include "vector3.h"


/*
Members or class Vector3
*/


EXPORT Vector3 Vector3::operator - () const
	{
	return Vector3(-X,-Y,-Z);
	}


EXPORT Vector3 Vector3::operator + (const Vector3& vector) const
	{
	return Vector3(X+vector.X,Y+vector.Y,Z+vector.Z);
	}


EXPORT Vector3 Vector3::operator - (const Vector3& vector) const
	{
	return Vector3(X-vector.X,Y-vector.Y,Z-vector.Z);
	}


EXPORT Vector3 Vector3::operator * (fix scalar) const
	{
	return Vector3(Fix::MulNS(X,scalar),Fix::MulNS(Y,scalar),Fix::MulNS(Z,scalar));
	}


EXPORT Vector3 Vector3::operator / (fix scalar) const
	{
	return Vector3(Fix::Div(X,scalar),Fix::Div(Y,scalar),Fix::Div(Z,scalar));
	}


EXPORT Vector3& Vector3::operator += (const Vector3& vector)
	{
	X += vector.X;
	Y += vector.Y;
	Z += vector.Z;
	return *this;
	}


EXPORT Vector3& Vector3::operator -= (const Vector3& vector)
	{
	X -= vector.X;
	Y -= vector.Y;
	Z -= vector.Z;
	return *this;
	}


EXPORT Vector3& Vector3::operator *= (fix scalar)
	{
	X = Fix::MulNS(X,scalar);
	Y = Fix::MulNS(Y,scalar);
	Z = Fix::MulNS(Z,scalar);
	return *this;
	}


EXPORT Vector3& Vector3::operator /= (fix scalar)
	{
	X = Fix::Div(X,scalar);
	Y = Fix::Div(Y,scalar);
	Z = Fix::Div(Z,scalar);
	return *this;
	}


EXPORT bool Vector3::operator == (const Vector3& vector) const
	{
	return (X==vector.X && Y==vector.Y && Z==vector.Z);
	}


EXPORT fix Vector3::DotProduct(const Vector3& vector) const
	{
	return Fix::MulNS(X,vector.X)+Fix::MulNS(Y,vector.Y)+Fix::MulNS(Z,vector.Z);
	}


EXPORT Vector3 Vector3::CrossProduct(const Vector3& vector) const
	{
	return Vector3(Fix::MulNS(Y,vector.Z)-Fix::MulNS(Z,vector.Y),
				   Fix::MulNS(Z,vector.X)-Fix::MulNS(X,vector.Z),
				   Fix::MulNS(X,vector.Y)-Fix::MulNS(Y,vector.X));
	}


static uint MostSignificantBit(uint32 a)
	{
	uint b = 0;
	if(a>=(1<<16)) b |= 16, a >>= 16;
	if(a>=(1<<8))  b |= 8,	a >>= 8;
	if(a>=(1<<4))  b |= 4,	a >>= 4;
	if(a>=(1<<2))  b |= 2,	a >>= 2;
	b |= a>>1;
	return b;
	}


EXPORT ufix Vector3::Length() const
	{
	// calculate square of the length
	uint f;
	uint i = LengthSquared(f);
	
	// if no integer part, return the square root
	if(!i)
		return Fix::Sqrt(f)>>8;

	// shift square of result so it fits into 32 bits
	int shift = (2+MostSignificantBit(i))&~1;
	ufix s;
	if(shift==32)
		s = i;
	else
		{
		s = f>>shift;
		s |= i<<(32-shift);
		}

	// take the square root
	ufix r = Fix::Sqrt(s);

	// shift result to get binary point in the correct place
	shift = (16-shift)>>1;
	if(shift>0)
		r >>= shift;
	if(shift<0)
		r <<= -shift;

	return r;
	}


EXPORT int Vector3::CompareLength(ufix length) const
	{
	// calculate the square of this vectors length as ai:af
	uint32 af;
	uint32 ai = LengthSquared(af);

	// calculate the square of 'length' as bi:bf
	uint32 l = length&0xFFFF;
	uint32 h = length>>16;
	uint32 bf = l*l;
	uint32 bi = h*h;
	uint32 r = l*h;
	uint32 obf = bf;
	bf += r<<(16+1);
	if(bf<obf) bi++;
	bi += r>>(16-1);

	// compare the squares of the length
	if(ai<bi)
		return -1;
	if(ai>bi)
		return 1;
	if(af<bf)
		return -1;
	if(af>bf)
		return 1;
	return 0;
	}


EXPORT int Vector3::CompareLengths(const Vector3& vector) const
	{
	uint32 af,bf;
	uint32 ai = LengthSquared(af);
	uint32 bi = vector.LengthSquared(bf);
	
	if(ai<bi)
		return -1;
	if(ai>bi)
		return 1;
	if(af<bf)
		return -1;
	if(af>bf)
		return 1;
	return 0;
	}


EXPORT uint32 Vector3::LengthSquared(uint32& fraction) const
	{
	int32	v,r;
	uint32	l,h,lo,hi,ol;
	
	v = X;
	l = v&0xFFFF;
	h = v>>16;
	lo = l*l;
	hi = h*h;
	r = l*h;
	ol = lo;
	lo += r<<(16+1);
	if(lo<ol) hi++;
	hi += r>>(16-1);
	
	v = Y;
	l = v&0xFFFF;
	h = v>>16;
	ol = lo;
	lo += l*l;
	if(lo<ol) hi++;
	hi += h*h;
	r = l*h;
	ol = lo;
	lo += r<<(16+1);
	if(lo<ol) hi++;
	hi += r>>(16-1);
	
	v = Z;
	l = v&0xFFFF;
	h = v>>16;
	ol = lo;
	lo += l*l;
	if(lo<ol) hi++;
	hi += h*h;
	r = l*h;
	ol = lo;
	lo += r<<(16+1);
	if(lo<ol) hi++;
	hi += r>>(16-1);

	fraction = lo;
	return hi;
	}


void Vector3::NormaliseComponents(uint bits)
	{
	// get components
	fix x = X;
	fix y = Y;
	fix z = Z;

	// find the magnitude of the largest component
	ufix max = x;
	if(x<0)
		max = ~x;

	ufix ay = y;
	if(y<0)
		ay = ~y;
	if(ay>max)
		max = ay;

	ufix az = z;
	if(z<0)
		az = ~z;
	if(az>max)
		max = az;

	// calculate shift value to get msb in correct place
	int shift = MostSignificantBit(max)-bits;

	// shift all components
	if(shift>0)
		{
		x >>= shift;
		y >>= shift;
		z >>= shift;
		}
	else if(shift<0)
		{
		shift = -shift;
		x <<= shift;
		y <<= shift;
		z <<= shift;
		}

	X = x;
	Y = y;
	Z = z;
	}


EXPORT Vector3 Vector3::UnitVector() const
	{
	// normalise component values to 15 bits
	Vector3 normalised(*this);
	normalised.NormaliseComponents(14);

	// calculate length of normalised vector
	fix x = normalised.X;
	fix y = normalised.Y;
	fix z = normalised.Z;
	ufix l = Fix::Sqrt(x*x+y*y+z*z);

	// divide components by length to get the unit vector
	x = Fix::Div(x<<8,l);
	y = Fix::Div(y<<8,l);
	z = Fix::Div(z<<8,l);

	return Vector3(x,y,z);
	}


EXPORT Vector3 Vector3::Normal(const Vector3& vector) const
	{
	// scale both vectors to 15 bits
	Vector3 a(*this);
	a.NormaliseComponents(14);
	Vector3 b(vector);
	b.NormaliseComponents(14);

	// calculate cross product (a vector normal to the two vectors)
	a = Vector3(a.Y*b.Z - a.Z*b.Y,
				a.Z*b.X - a.X*b.Z,
				a.X*b.Y - a.Y*b.X);

	// return the normal as a length 1.0 unit vector
	return a.UnitVector();
	}


EXPORT fixangle Vector3::Angle(const Vector3& vector) const
	{
	return Fix::ACos(UnitVector().DotProduct(vector.UnitVector()));
	}


EXPORT Vector3 Vector3::Normal(const Vector3& point1,const Vector3& point2) const
	{
	return (point1-*this).Normal(point2-*this);
	}


EXPORT fixangle Vector3::Angle(const Vector3& point1,const Vector3& point2) const
	{
	return (point1-*this).Angle(point2-*this);
	}


EXPORT void Vector3::Translate(Vector3* outVectors,uint vectorCount,const Vector3* inVectors,const Vector3& offset)
	{
	fix x = offset.X;
	fix y = offset.Y;
	fix z = offset.Z;
	Vector3* end = outVectors+vectorCount;
	while(outVectors<end)
		{
		outVectors->X = inVectors->X+x;
		outVectors->Y = inVectors->Y+y;
		outVectors->Z = inVectors->Z+z;
		++inVectors;
		++outVectors;
		}
	}


EXPORT void Vector3::Scale(Vector3* outVectors,uint vectorCount,const Vector3* inVectors,fix scale)
	{
	Vector3* end = outVectors+vectorCount;
	while(outVectors<end)
		{
		outVectors->X = Fix::MulNS(inVectors->X,scale);
		outVectors->Y = Fix::MulNS(inVectors->Y,scale);
		outVectors->Z = Fix::MulNS(inVectors->Z,scale);
		++inVectors;
		++outVectors;
		}
	}


/*
Members of class Matrix3
*/


EXPORT Vector3 Matrix3::operator * (const Vector3& vector) const
	{
	fix ax = vector.X;
	fix ay = vector.Y;
	fix az = vector.Z;
	
	fix rx = Fix::MulNS(ax,Row1.X)+Fix::MulNS(ay,Row1.Y)+Fix::MulNS(az,Row1.Z);
	fix ry = Fix::MulNS(ax,Row2.X)+Fix::MulNS(ay,Row2.Y)+Fix::MulNS(az,Row2.Z);
	fix rz = Fix::MulNS(ax,Row3.X)+Fix::MulNS(ay,Row3.Y)+Fix::MulNS(az,Row3.Z);
	
	return Vector3(rx,ry,rz);
	}


EXPORT Matrix3 Matrix3::Transposition() const
	{
	return Matrix3(Vector3(Row1.X,Row2.X,Row3.X),
				   Vector3(Row1.Y,Row2.Y,Row3.Y),
				   Vector3(Row1.Z,Row2.Z,Row3.Z));
	}


EXPORT void Matrix3::Transform(Vector3* outVectors,uint vectorCount,const Vector3* inVectors)
	{
	Vector3* end = outVectors+vectorCount;
	while(outVectors<end)
		*outVectors++ = *this * *inVectors++;
	}