hello world

neo/idlib/math/Angles.cpp

@@ -0,0 +1,240 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+#include <float.h>
+
+idAngles ang_zero( 0.0f, 0.0f, 0.0f );
+
+
+/*
+=================
+idAngles::Normalize360
+
+returns angles normalized to the range [0 <= angle < 360]
+=================
+*/
+idAngles& idAngles::Normalize360( void ) {
+	int i;
+
+	for ( i = 0; i < 3; i++ ) {
+		if ( ( (*this)[i] >= 360.0f ) || ( (*this)[i] < 0.0f ) ) {
+			(*this)[i] -= floor( (*this)[i] / 360.0f ) * 360.0f;
+
+			if ( (*this)[i] >= 360.0f ) {
+				(*this)[i] -= 360.0f;
+			}
+			if ( (*this)[i] < 0.0f ) {
+				(*this)[i] += 360.0f;
+			}
+		}
+	}
+
+	return *this;
+}
+
+/*
+=================
+idAngles::Normalize180
+
+returns angles normalized to the range [-180 < angle <= 180]
+=================
+*/
+idAngles& idAngles::Normalize180( void ) {
+	Normalize360();
+
+	if ( pitch > 180.0f ) {
+		pitch -= 360.0f;
+	}
+	
+	if ( yaw > 180.0f ) {
+		yaw -= 360.0f;
+	}
+
+	if ( roll > 180.0f ) {
+		roll -= 360.0f;
+	}
+	return *this;
+}
+
+/*
+=================
+idAngles::ToVectors
+=================
+*/
+void idAngles::ToVectors( idVec3 *forward, idVec3 *right, idVec3 *up ) const {
+	float sr, sp, sy, cr, cp, cy;
+	
+	idMath::SinCos( DEG2RAD( yaw ), sy, cy );
+	idMath::SinCos( DEG2RAD( pitch ), sp, cp );
+	idMath::SinCos( DEG2RAD( roll ), sr, cr );
+
+	if ( forward ) {
+		forward->Set( cp * cy, cp * sy, -sp );
+	}
+
+	if ( right ) {
+		right->Set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
+	}
+
+	if ( up ) {
+		up->Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
+	}
+}
+
+/*
+=================
+idAngles::ToForward
+=================
+*/
+idVec3 idAngles::ToForward( void ) const {
+	float sp, sy, cp, cy;
+	
+	idMath::SinCos( DEG2RAD( yaw ), sy, cy );
+	idMath::SinCos( DEG2RAD( pitch ), sp, cp );
+
+	return idVec3( cp * cy, cp * sy, -sp );
+}
+
+/*
+=================
+idAngles::ToQuat
+=================
+*/
+idQuat idAngles::ToQuat( void ) const {
+	float sx, cx, sy, cy, sz, cz;
+	float sxcy, cxcy, sxsy, cxsy;
+
+	idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
+	idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
+	idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
+
+	sxcy = sx * cy;
+	cxcy = cx * cy;
+	sxsy = sx * sy;
+	cxsy = cx * sy;
+
+	return idQuat( cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz );
+}
+
+/*
+=================
+idAngles::ToRotation
+=================
+*/
+idRotation idAngles::ToRotation( void ) const {
+	idVec3 vec;
+	float angle, w;
+	float sx, cx, sy, cy, sz, cz;
+	float sxcy, cxcy, sxsy, cxsy;
+
+	if ( pitch == 0.0f ) {
+		if ( yaw == 0.0f ) {
+			return idRotation( vec3_origin, idVec3( -1.0f, 0.0f, 0.0f ), roll );
+		}
+		if ( roll == 0.0f ) {
+			return idRotation( vec3_origin, idVec3( 0.0f, 0.0f, -1.0f ), yaw );
+		}
+	} else if ( yaw == 0.0f && roll == 0.0f ) {
+		return idRotation( vec3_origin, idVec3( 0.0f, -1.0f, 0.0f ), pitch );
+	}
+
+	idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
+	idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
+	idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
+
+	sxcy = sx * cy;
+	cxcy = cx * cy;
+	sxsy = sx * sy;
+	cxsy = cx * sy;
+
+	vec.x =  cxsy * sz - sxcy * cz;
+	vec.y = -cxsy * cz - sxcy * sz;
+	vec.z =  sxsy * cz - cxcy * sz;
+	w =		 cxcy * cz + sxsy * sz;
+	angle = idMath::ACos( w );
+	if ( angle == 0.0f ) {
+		vec.Set( 0.0f, 0.0f, 1.0f );
+	} else {
+		//vec *= (1.0f / sin( angle ));
+		vec.Normalize();
+		vec.FixDegenerateNormal();
+		angle *= 2.0f * idMath::M_RAD2DEG;
+	}
+	return idRotation( vec3_origin, vec, angle );
+}
+
+/*
+=================
+idAngles::ToMat3
+=================
+*/
+idMat3 idAngles::ToMat3( void ) const {
+	idMat3 mat;
+	float sr, sp, sy, cr, cp, cy;
+
+	idMath::SinCos( DEG2RAD( yaw ), sy, cy );
+	idMath::SinCos( DEG2RAD( pitch ), sp, cp );
+	idMath::SinCos( DEG2RAD( roll ), sr, cr );
+
+	mat[ 0 ].Set( cp * cy, cp * sy, -sp );
+	mat[ 1 ].Set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
+	mat[ 2 ].Set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
+
+	return mat;
+}
+
+/*
+=================
+idAngles::ToMat4
+=================
+*/
+idMat4 idAngles::ToMat4( void ) const {
+	return ToMat3().ToMat4();
+}
+
+/*
+=================
+idAngles::ToAngularVelocity
+=================
+*/
+idVec3 idAngles::ToAngularVelocity( void ) const {
+	idRotation rotation = idAngles::ToRotation();
+	return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
+}
+
+/*
+=============
+idAngles::ToString
+=============
+*/
+const char *idAngles::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}

neo/idlib/math/Angles.h

@@ -0,0 +1,262 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_ANGLES_H__
+#define __MATH_ANGLES_H__
+
+/*
+===============================================================================
+
+	Euler angles
+
+===============================================================================
+*/
+
+// angle indexes
+#define	PITCH				0		// up / down
+#define	YAW					1		// left / right
+#define	ROLL				2		// fall over
+
+class idVec3;
+class idQuat;
+class idRotation;
+class idMat3;
+class idMat4;
+
+class idAngles {
+public:
+	float			pitch;
+	float			yaw;
+	float			roll;
+
+					idAngles( void );
+					idAngles( float pitch, float yaw, float roll );
+					explicit idAngles( const idVec3 &v );
+
+	void 			Set( float pitch, float yaw, float roll );
+	idAngles &		Zero( void );
+
+	float			operator[]( int index ) const;
+	float &			operator[]( int index );
+	idAngles		operator-() const;			// negate angles, in general not the inverse rotation
+	idAngles &		operator=( const idAngles &a );
+	idAngles		operator+( const idAngles &a ) const;
+	idAngles &		operator+=( const idAngles &a );
+	idAngles		operator-( const idAngles &a ) const;
+	idAngles &		operator-=( const idAngles &a );
+	idAngles		operator*( const float a ) const;
+	idAngles &		operator*=( const float a );
+	idAngles		operator/( const float a ) const;
+	idAngles &		operator/=( const float a );
+
+	friend idAngles	operator*( const float a, const idAngles &b );
+
+	bool			Compare( const idAngles &a ) const;							// exact compare, no epsilon
+	bool			Compare( const idAngles &a, const float epsilon ) const;	// compare with epsilon
+	bool			operator==(	const idAngles &a ) const;						// exact compare, no epsilon
+	bool			operator!=(	const idAngles &a ) const;						// exact compare, no epsilon
+
+	idAngles &		Normalize360( void );	// normalizes 'this'
+	idAngles &		Normalize180( void );	// normalizes 'this'
+
+	void			Clamp( const idAngles &min, const idAngles &max );
+
+	int				GetDimension( void ) const;
+
+	void			ToVectors( idVec3 *forward, idVec3 *right = NULL, idVec3 *up = NULL ) const;
+	idVec3			ToForward( void ) const;
+	idQuat			ToQuat( void ) const;
+	idRotation		ToRotation( void ) const;
+	idMat3			ToMat3( void ) const;
+	idMat4			ToMat4( void ) const;
+	idVec3			ToAngularVelocity( void ) const;
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+};
+
+extern idAngles ang_zero;
+
+ID_INLINE idAngles::idAngles( void ) {
+}
+
+ID_INLINE idAngles::idAngles( float pitch, float yaw, float roll ) {
+	this->pitch = pitch;
+	this->yaw	= yaw;
+	this->roll	= roll;
+}
+
+ID_INLINE idAngles::idAngles( const idVec3 &v ) {
+	this->pitch = v[0];
+	this->yaw	= v[1];
+	this->roll	= v[2];
+}
+
+ID_INLINE void idAngles::Set( float pitch, float yaw, float roll ) {
+	this->pitch = pitch;
+	this->yaw	= yaw;
+	this->roll	= roll;
+}
+
+ID_INLINE idAngles &idAngles::Zero( void ) {
+	pitch = yaw = roll = 0.0f;
+	return *this;
+}
+
+ID_INLINE float idAngles::operator[]( int index ) const {
+	assert( ( index >= 0 ) && ( index < 3 ) );
+	return ( &pitch )[ index ];
+}
+
+ID_INLINE float &idAngles::operator[]( int index ) {
+	assert( ( index >= 0 ) && ( index < 3 ) );
+	return ( &pitch )[ index ];
+}
+
+ID_INLINE idAngles idAngles::operator-() const {
+	return idAngles( -pitch, -yaw, -roll );
+}
+
+ID_INLINE idAngles &idAngles::operator=( const idAngles &a ) {
+	pitch	= a.pitch;
+	yaw		= a.yaw;
+	roll	= a.roll;
+	return *this;
+}
+
+ID_INLINE idAngles idAngles::operator+( const idAngles &a ) const {
+	return idAngles( pitch + a.pitch, yaw + a.yaw, roll + a.roll );
+}
+
+ID_INLINE idAngles& idAngles::operator+=( const idAngles &a ) {
+	pitch	+= a.pitch;
+	yaw		+= a.yaw;
+	roll	+= a.roll;
+
+	return *this;
+}
+
+ID_INLINE idAngles idAngles::operator-( const idAngles &a ) const {
+	return idAngles( pitch - a.pitch, yaw - a.yaw, roll - a.roll );
+}
+
+ID_INLINE idAngles& idAngles::operator-=( const idAngles &a ) {
+	pitch	-= a.pitch;
+	yaw		-= a.yaw;
+	roll	-= a.roll;
+
+	return *this;
+}
+
+ID_INLINE idAngles idAngles::operator*( const float a ) const {
+	return idAngles( pitch * a, yaw * a, roll * a );
+}
+
+ID_INLINE idAngles& idAngles::operator*=( float a ) {
+	pitch	*= a;
+	yaw		*= a;
+	roll	*= a;
+	return *this;
+}
+
+ID_INLINE idAngles idAngles::operator/( const float a ) const {
+	float inva = 1.0f / a;
+	return idAngles( pitch * inva, yaw * inva, roll * inva );
+}
+
+ID_INLINE idAngles& idAngles::operator/=( float a ) {
+	float inva = 1.0f / a;
+	pitch	*= inva;
+	yaw		*= inva;
+	roll	*= inva;
+	return *this;
+}
+
+ID_INLINE idAngles operator*( const float a, const idAngles &b ) {
+	return idAngles( a * b.pitch, a * b.yaw, a * b.roll );
+}
+
+ID_INLINE bool idAngles::Compare( const idAngles &a ) const {
+	return ( ( a.pitch == pitch ) && ( a.yaw == yaw ) && ( a.roll == roll ) );
+}
+
+ID_INLINE bool idAngles::Compare( const idAngles &a, const float epsilon ) const {
+	if ( idMath::Fabs( pitch - a.pitch ) > epsilon ) {
+		return false;
+	}
+			
+	if ( idMath::Fabs( yaw - a.yaw ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( roll - a.roll ) > epsilon ) {
+		return false;
+	}
+
+	return true;
+}
+
+ID_INLINE bool idAngles::operator==( const idAngles &a ) const {
+	return Compare( a );
+}
+
+ID_INLINE bool idAngles::operator!=( const idAngles &a ) const {
+	return !Compare( a );
+}
+
+ID_INLINE void idAngles::Clamp( const idAngles &min, const idAngles &max ) {
+	if ( pitch < min.pitch ) {
+		pitch = min.pitch;
+	} else if ( pitch > max.pitch ) {
+		pitch = max.pitch;
+	}
+	if ( yaw < min.yaw ) {
+		yaw = min.yaw;
+	} else if ( yaw > max.yaw ) {
+		yaw = max.yaw;
+	}
+	if ( roll < min.roll ) {
+		roll = min.roll;
+	} else if ( roll > max.roll ) {
+		roll = max.roll;
+	}
+}
+
+ID_INLINE int idAngles::GetDimension( void ) const {
+	return 3;
+}
+
+ID_INLINE const float *idAngles::ToFloatPtr( void ) const {
+	return &pitch;
+}
+
+ID_INLINE float *idAngles::ToFloatPtr( void ) {
+	return &pitch;
+}
+
+#endif /* !__MATH_ANGLES_H__ */

neo/idlib/math/Complex.cpp

@@ -0,0 +1,41 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+idComplex complex_origin( 0.0f, 0.0f );
+
+/*
+=============
+idComplex::ToString
+=============
+*/
+const char *idComplex::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}

neo/idlib/math/Complex.h

@@ -0,0 +1,348 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_COMPLEX_H__
+#define __MATH_COMPLEX_H__
+
+/*
+===============================================================================
+
+  Complex number
+
+===============================================================================
+*/
+
+class idComplex {
+public:
+	float				r;		// real part
+	float				i;		// imaginary part
+
+						idComplex( void );
+						idComplex( const float r, const float i );
+
+	void 				Set( const float r, const float i );
+	void				Zero( void );
+
+	float				operator[]( int index ) const;
+	float &				operator[]( int index );
+
+	idComplex			operator-() const;
+	idComplex &			operator=( const idComplex &a );
+
+	idComplex			operator*( const idComplex &a ) const;
+	idComplex			operator/( const idComplex &a ) const;
+	idComplex			operator+( const idComplex &a ) const;
+	idComplex			operator-( const idComplex &a ) const;
+
+	idComplex &			operator*=( const idComplex &a );
+	idComplex &			operator/=( const idComplex &a );
+	idComplex &			operator+=( const idComplex &a );
+	idComplex &			operator-=( const idComplex &a );
+
+	idComplex			operator*( const float a ) const;
+	idComplex			operator/( const float a ) const;
+	idComplex			operator+( const float a ) const;
+	idComplex			operator-( const float a ) const;
+
+	idComplex &			operator*=( const float a );
+	idComplex &			operator/=( const float a );
+	idComplex &			operator+=( const float a );
+	idComplex &			operator-=( const float a );
+
+	friend idComplex	operator*( const float a, const idComplex &b );
+	friend idComplex	operator/( const float a, const idComplex &b );
+	friend idComplex	operator+( const float a, const idComplex &b );
+	friend idComplex	operator-( const float a, const idComplex &b );
+
+	bool				Compare( const idComplex &a ) const;						// exact compare, no epsilon
+	bool				Compare( const idComplex &a, const float epsilon ) const;	// compare with epsilon
+	bool				operator==(	const idComplex &a ) const;						// exact compare, no epsilon
+	bool				operator!=(	const idComplex &a ) const;						// exact compare, no epsilon
+
+	idComplex			Reciprocal( void ) const;
+	idComplex			Sqrt( void ) const;
+	float				Abs( void ) const;
+
+	int					GetDimension( void ) const;
+
+	const float *		ToFloatPtr( void ) const;
+	float *				ToFloatPtr( void );
+	const char *		ToString( int precision = 2 ) const;
+};
+
+extern idComplex complex_origin;
+#define complex_zero complex_origin
+
+ID_INLINE idComplex::idComplex( void ) {
+}
+
+ID_INLINE idComplex::idComplex( const float r, const float i ) {
+	this->r = r;
+	this->i = i;
+}
+
+ID_INLINE void idComplex::Set( const float r, const float i ) {
+	this->r = r;
+	this->i = i;
+}
+
+ID_INLINE void idComplex::Zero( void ) {
+	r = i = 0.0f;
+}
+
+ID_INLINE float idComplex::operator[]( int index ) const {
+	assert( index >= 0 && index < 2 );
+	return ( &r )[ index ];
+}
+
+ID_INLINE float& idComplex::operator[]( int index ) {
+	assert( index >= 0 && index < 2 );
+	return ( &r )[ index ];
+}
+
+ID_INLINE idComplex idComplex::operator-() const {
+	return idComplex( -r, -i );
+}
+
+ID_INLINE idComplex &idComplex::operator=( const idComplex &a ) {
+	r = a.r;
+	i = a.i;
+	return *this;
+}
+
+ID_INLINE idComplex idComplex::operator*( const idComplex &a ) const {
+	return idComplex( r * a.r - i * a.i, i * a.r + r * a.i );
+}
+
+ID_INLINE idComplex idComplex::operator/( const idComplex &a ) const {
+	float s, t;
+	if ( idMath::Fabs( a.r ) >= idMath::Fabs( a.i ) ) {
+		s = a.i / a.r;
+		t = 1.0f / ( a.r + s * a.i );
+		return idComplex( ( r + s * i ) * t, ( i - s * r ) * t );
+	} else {
+		s = a.r / a.i;
+		t = 1.0f / ( s * a.r + a.i );
+		return idComplex( ( r * s + i ) * t, ( i * s - r ) * t );
+	}
+}
+
+ID_INLINE idComplex idComplex::operator+( const idComplex &a ) const {
+	return idComplex( r + a.r, i + a.i );
+}
+
+ID_INLINE idComplex idComplex::operator-( const idComplex &a ) const {
+	return idComplex( r - a.r, i - a.i );
+}
+
+ID_INLINE idComplex &idComplex::operator*=( const idComplex &a ) {
+	*this = idComplex( r * a.r - i * a.i, i * a.r + r * a.i );
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator/=( const idComplex &a ) {
+	float s, t;
+	if ( idMath::Fabs( a.r ) >= idMath::Fabs( a.i ) ) {
+		s = a.i / a.r;
+		t = 1.0f / ( a.r + s * a.i );
+		*this = idComplex( ( r + s * i ) * t, ( i - s * r ) * t );
+	} else {
+		s = a.r / a.i;
+		t = 1.0f / ( s * a.r + a.i );
+		*this = idComplex( ( r * s + i ) * t, ( i * s - r ) * t );
+	}
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator+=( const idComplex &a ) {
+	r += a.r;
+	i += a.i;
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator-=( const idComplex &a ) {
+	r -= a.r;
+	i -= a.i;
+	return *this;
+}
+
+ID_INLINE idComplex idComplex::operator*( const float a ) const {
+	return idComplex( r * a, i * a );
+}
+
+ID_INLINE idComplex idComplex::operator/( const float a ) const {
+	float s = 1.0f / a;
+	return idComplex( r * s, i * s );
+}
+
+ID_INLINE idComplex idComplex::operator+( const float a ) const {
+	return idComplex( r + a, i );
+}
+
+ID_INLINE idComplex idComplex::operator-( const float a ) const {
+	return idComplex( r - a, i );
+}
+
+ID_INLINE idComplex &idComplex::operator*=( const float a ) {
+	r *= a;
+	i *= a;
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator/=( const float a ) {
+	float s = 1.0f / a;
+	r *= s;
+	i *= s;
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator+=( const float a ) {
+	r += a;
+	return *this;
+}
+
+ID_INLINE idComplex &idComplex::operator-=( const float a ) {
+	r -= a;
+	return *this;
+}
+
+ID_INLINE idComplex operator*( const float a, const idComplex &b ) {
+	return idComplex( a * b.r, a * b.i );
+}
+
+ID_INLINE idComplex operator/( const float a, const idComplex &b ) {
+	float s, t;
+	if ( idMath::Fabs( b.r ) >= idMath::Fabs( b.i ) ) {
+		s = b.i / b.r;
+		t = a / ( b.r + s * b.i );
+		return idComplex( t, - s * t );
+	} else {
+		s = b.r / b.i;
+		t = a / ( s * b.r + b.i );
+		return idComplex( s * t, - t );
+	}
+}
+
+ID_INLINE idComplex operator+( const float a, const idComplex &b ) {
+	return idComplex( a + b.r, b.i );
+}
+
+ID_INLINE idComplex operator-( const float a, const idComplex &b ) {
+	return idComplex( a - b.r, -b.i );
+}
+
+ID_INLINE idComplex idComplex::Reciprocal( void ) const {
+	float s, t;
+	if ( idMath::Fabs( r ) >= idMath::Fabs( i ) ) {
+		s = i / r;
+		t = 1.0f / ( r + s * i );
+		return idComplex( t, - s * t );
+	} else {
+		s = r / i;
+		t = 1.0f / ( s * r + i );
+		return idComplex( s * t, - t );
+	}
+}
+
+ID_INLINE idComplex idComplex::Sqrt( void ) const {
+	float x, y, w;
+
+	if ( r == 0.0f && i == 0.0f ) {
+		return idComplex( 0.0f, 0.0f );
+	}
+	x = idMath::Fabs( r );
+	y = idMath::Fabs( i );
+	if ( x >= y ) {
+		w = y / x;
+		w = idMath::Sqrt( x ) * idMath::Sqrt( 0.5f * ( 1.0f + idMath::Sqrt( 1.0f + w * w ) ) );
+	} else {
+		w = x / y;
+		w = idMath::Sqrt( y ) * idMath::Sqrt( 0.5f * ( w + idMath::Sqrt( 1.0f + w * w ) ) );
+	}
+	if ( w == 0.0f ) {
+		return idComplex( 0.0f, 0.0f );
+	}
+	if ( r >= 0.0f ) {
+		return idComplex( w, 0.5f * i / w );
+	} else {
+		return idComplex( 0.5f * y / w, ( i >= 0.0f ) ? w : -w );
+	}
+}
+
+ID_INLINE float idComplex::Abs( void ) const {
+	float x, y, t;
+	x = idMath::Fabs( r );
+	y = idMath::Fabs( i );
+	if ( x == 0.0f ) {
+		return y;
+	} else if ( y == 0.0f ) {
+		return x;
+	} else if ( x > y ) {
+		t = y / x;
+		return x * idMath::Sqrt( 1.0f + t * t );
+	} else {
+		t = x / y;
+		return y * idMath::Sqrt( 1.0f + t * t );
+	}
+}
+
+ID_INLINE bool idComplex::Compare( const idComplex &a ) const {
+	return ( ( r == a.r ) && ( i == a.i ) );
+}
+
+ID_INLINE bool idComplex::Compare( const idComplex &a, const float epsilon ) const {
+	if ( idMath::Fabs( r - a.r ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( i - a.i ) > epsilon ) {
+		return false;
+	}
+	return true;
+}
+
+ID_INLINE bool idComplex::operator==( const idComplex &a ) const {
+	return Compare( a );
+}
+
+ID_INLINE bool idComplex::operator!=( const idComplex &a ) const {
+	return !Compare( a );
+}
+
+ID_INLINE int idComplex::GetDimension( void ) const {
+	return 2;
+}
+
+ID_INLINE const float *idComplex::ToFloatPtr( void ) const {
+	return &r;
+}
+
+ID_INLINE float *idComplex::ToFloatPtr( void ) {
+	return &r;
+}
+
+#endif /* !__MATH_COMPLEX_H__ */

neo/idlib/math/Extrapolate.h

@@ -0,0 +1,241 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_EXTRAPOLATE_H__
+#define __MATH_EXTRAPOLATE_H__
+
+/*
+==============================================================================================
+
+	Extrapolation
+
+==============================================================================================
+*/
+
+typedef enum {
+	EXTRAPOLATION_NONE			= 0x01,	// no extrapolation, covered distance = duration * 0.001 * ( baseSpeed )
+	EXTRAPOLATION_LINEAR		= 0x02,	// linear extrapolation, covered distance = duration * 0.001 * ( baseSpeed + speed )
+	EXTRAPOLATION_ACCELLINEAR	= 0x04,	// linear acceleration, covered distance = duration * 0.001 * ( baseSpeed + 0.5 * speed )
+	EXTRAPOLATION_DECELLINEAR	= 0x08,	// linear deceleration, covered distance = duration * 0.001 * ( baseSpeed + 0.5 * speed )
+	EXTRAPOLATION_ACCELSINE		= 0x10,	// sinusoidal acceleration, covered distance = duration * 0.001 * ( baseSpeed + sqrt( 0.5 ) * speed )
+	EXTRAPOLATION_DECELSINE		= 0x20,	// sinusoidal deceleration, covered distance = duration * 0.001 * ( baseSpeed + sqrt( 0.5 ) * speed )
+	EXTRAPOLATION_NOSTOP		= 0x40	// do not stop at startTime + duration
+} extrapolation_t;
+
+template< class type >
+class idExtrapolate {
+public:
+						idExtrapolate();
+
+	void				Init( const float startTime, const float duration, const type &startValue, const type &baseSpeed, const type &speed, const extrapolation_t extrapolationType );
+	type				GetCurrentValue( float time ) const;
+	type				GetCurrentSpeed( float time ) const;
+	bool				IsDone( float time ) const { return ( !( extrapolationType & EXTRAPOLATION_NOSTOP ) && time >= startTime + duration ); }
+	void				SetStartTime( float time ) { startTime = time; currentTime = -1; }
+	float				GetStartTime( void ) const { return startTime; }
+	float				GetEndTime( void ) const { return ( !( extrapolationType & EXTRAPOLATION_NOSTOP ) && duration > 0 ) ? startTime + duration : 0; }
+	float				GetDuration( void ) const { return duration; }
+	void				SetStartValue( const type &value ) { startValue = value; currentTime = -1; }
+	const type &		GetStartValue( void ) const { return startValue; }
+	const type &		GetBaseSpeed( void ) const { return baseSpeed; }
+	const type &		GetSpeed( void ) const { return speed; }
+	extrapolation_t		GetExtrapolationType( void ) const { return extrapolationType; }
+
+private:
+	extrapolation_t		extrapolationType;
+	float				startTime;
+	float				duration;
+	type				startValue;
+	type				baseSpeed;
+	type				speed;
+	mutable float		currentTime;
+	mutable type		currentValue;
+};
+
+/*
+====================
+idExtrapolate::idExtrapolate
+====================
+*/
+template< class type >
+ID_INLINE idExtrapolate<type>::idExtrapolate() {
+	extrapolationType = EXTRAPOLATION_NONE;
+	startTime = duration = 0.0f;
+	memset( &startValue, 0, sizeof( startValue ) );
+	memset( &baseSpeed, 0, sizeof( baseSpeed ) );
+	memset( &speed, 0, sizeof( speed ) );
+	currentTime = -1;
+	currentValue = startValue;
+}
+
+/*
+====================
+idExtrapolate::Init
+====================
+*/
+template< class type >
+ID_INLINE void idExtrapolate<type>::Init( const float startTime, const float duration, const type &startValue, const type &baseSpeed, const type &speed, const extrapolation_t extrapolationType ) {
+	this->extrapolationType = extrapolationType;
+	this->startTime = startTime;
+	this->duration = duration;
+	this->startValue = startValue;
+	this->baseSpeed = baseSpeed;
+	this->speed = speed;
+	currentTime = -1;
+	currentValue = startValue;
+}
+
+/*
+====================
+idExtrapolate::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idExtrapolate<type>::GetCurrentValue( float time ) const {
+	float deltaTime, s;
+
+	if ( time == currentTime ) {
+		return currentValue;
+	}
+
+	currentTime = time;
+
+	if ( time < startTime ) {
+		return startValue;
+	}
+
+	if ( !( extrapolationType &	EXTRAPOLATION_NOSTOP ) && ( time > startTime + duration ) ) {
+		time = startTime + duration;
+	}
+
+	switch( extrapolationType & ~EXTRAPOLATION_NOSTOP ) {
+		case EXTRAPOLATION_NONE: {
+			deltaTime = ( time - startTime ) * 0.001f;
+			currentValue = startValue + deltaTime * baseSpeed;
+			break;
+		}
+		case EXTRAPOLATION_LINEAR: {
+			deltaTime = ( time - startTime ) * 0.001f;
+			currentValue = startValue + deltaTime * ( baseSpeed + speed );
+			break;
+		}
+		case EXTRAPOLATION_ACCELLINEAR: {
+			if ( !duration ) {
+				currentValue = startValue;
+			} else {
+				deltaTime = ( time - startTime ) / duration;
+				s = ( 0.5f * deltaTime * deltaTime ) * ( duration * 0.001f );
+				currentValue = startValue + deltaTime * baseSpeed + s * speed;
+			}
+			break;
+		}
+		case EXTRAPOLATION_DECELLINEAR: {
+			if ( !duration ) {
+				currentValue = startValue;
+			} else {
+				deltaTime = ( time - startTime ) / duration;
+				s = ( deltaTime - ( 0.5f * deltaTime * deltaTime ) ) * ( duration * 0.001f );
+				currentValue = startValue + deltaTime * baseSpeed + s * speed;
+			}
+			break;
+		}
+		case EXTRAPOLATION_ACCELSINE: {
+			if ( !duration ) {
+				currentValue = startValue;
+			} else {
+				deltaTime = ( time - startTime ) / duration;
+				s = ( 1.0f - idMath::Cos( deltaTime * idMath::HALF_PI ) ) * duration * 0.001f * idMath::SQRT_1OVER2;
+				currentValue = startValue + deltaTime * baseSpeed + s * speed;
+			}
+			break;
+		}
+		case EXTRAPOLATION_DECELSINE: {
+			if ( !duration ) {
+				currentValue = startValue;
+			} else {
+				deltaTime = ( time - startTime ) / duration;
+				s = idMath::Sin( deltaTime * idMath::HALF_PI ) * duration * 0.001f * idMath::SQRT_1OVER2;
+				currentValue = startValue + deltaTime * baseSpeed + s * speed;
+			}
+			break;
+		}
+	}
+	return currentValue;
+}
+
+/*
+====================
+idExtrapolate::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idExtrapolate<type>::GetCurrentSpeed( float time ) const {
+	float deltaTime, s;
+
+	if ( time < startTime || !duration ) {
+		return ( startValue - startValue );
+	}
+
+	if ( !( extrapolationType &	EXTRAPOLATION_NOSTOP ) && ( time > startTime + duration ) ) {
+		return ( startValue - startValue );
+	}
+
+	switch( extrapolationType & ~EXTRAPOLATION_NOSTOP ) {
+		case EXTRAPOLATION_NONE: {
+			return baseSpeed;
+		}
+		case EXTRAPOLATION_LINEAR: {
+			return baseSpeed + speed;
+		}
+		case EXTRAPOLATION_ACCELLINEAR: {
+			deltaTime = ( time - startTime ) / duration;
+			s = deltaTime;
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_DECELLINEAR: {
+			deltaTime = ( time - startTime ) / duration;
+			s = 1.0f - deltaTime;
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_ACCELSINE: {
+			deltaTime = ( time - startTime ) / duration;
+			s = idMath::Sin( deltaTime * idMath::HALF_PI );
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_DECELSINE: {
+			deltaTime = ( time - startTime ) / duration;
+			s = idMath::Cos( deltaTime * idMath::HALF_PI );
+			return baseSpeed + s * speed;
+		}
+		default: {
+			return baseSpeed;
+		}
+	}
+}
+
+#endif /* !__MATH_EXTRAPOLATE_H__ */

neo/idlib/math/Interpolate.h

@@ -0,0 +1,417 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_INTERPOLATE_H__
+#define __MATH_INTERPOLATE_H__
+
+/*
+==============================================================================================
+
+	Linear interpolation.
+
+==============================================================================================
+*/
+
+template< class type >
+class idInterpolate {
+public:
+						idInterpolate();
+
+	void				Init( const float startTime, const float duration, const type &startValue, const type &endValue );
+	void				SetStartTime( float time ) { this->startTime = time; }
+	void				SetDuration( float duration ) { this->duration = duration; }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; }
+
+	type				GetCurrentValue( float time ) const;
+	bool				IsDone( float time ) const { return ( time >= startTime + duration ); }
+
+	float				GetStartTime( void ) const { return startTime; }
+	float				GetEndTime( void ) const { return startTime + duration; }
+	float				GetDuration( void ) const { return duration; }
+	const type &		GetStartValue( void ) const { return startValue; }
+	const type &		GetEndValue( void ) const { return endValue; }
+
+private:
+	float				startTime;
+	float				duration;
+	type				startValue;
+	type				endValue;
+	mutable float		currentTime;
+	mutable type		currentValue;
+};
+
+/*
+====================
+idInterpolate::idInterpolate
+====================
+*/
+template< class type >
+ID_INLINE idInterpolate<type>::idInterpolate() {
+	currentTime = startTime = duration = 0;
+	memset( &currentValue, 0, sizeof( currentValue ) );
+	startValue = endValue = currentValue;
+}
+
+/*
+====================
+idInterpolate::Init
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolate<type>::Init( const float startTime, const float duration, const type &startValue, const type &endValue ) {
+	this->startTime = startTime;
+	this->duration = duration;
+	this->startValue = startValue;
+	this->endValue = endValue;
+	this->currentTime = startTime - 1;
+	this->currentValue = startValue;
+}
+
+/*
+====================
+idInterpolate::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolate<type>::GetCurrentValue( float time ) const {
+	float deltaTime;
+
+	deltaTime = time - startTime;
+	if ( time != currentTime ) {
+		currentTime = time;
+		if ( deltaTime <= 0 ) {
+			currentValue = startValue;
+		} else if ( deltaTime >= duration ) {
+			currentValue = endValue;
+		} else {
+			currentValue = startValue + ( endValue - startValue ) * ( (float) deltaTime / duration );
+		}
+	}
+	return currentValue;
+}
+
+
+/*
+==============================================================================================
+
+	Continuous interpolation with linear acceleration and deceleration phase.
+	The velocity is continuous but the acceleration is not.
+
+==============================================================================================
+*/
+
+template< class type >
+class idInterpolateAccelDecelLinear  {
+public:
+						idInterpolateAccelDecelLinear();
+
+	void				Init( const float startTime, const float accelTime, const float decelTime, const float duration, const type &startValue, const type &endValue );
+	void				SetStartTime( float time ) { startTime = time; Invalidate(); }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; Invalidate(); }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; Invalidate(); }
+
+	type				GetCurrentValue( float time ) const;
+	type				GetCurrentSpeed( float time ) const;
+	bool				IsDone( float time ) const { return ( time >= startTime + accelTime + linearTime + decelTime ); }
+
+	float				GetStartTime( void ) const { return startTime; }
+	float				GetEndTime( void ) const { return startTime + accelTime + linearTime + decelTime; }
+	float				GetDuration( void ) const { return accelTime + linearTime + decelTime; }
+	float				GetAcceleration( void ) const { return accelTime; }
+	float				GetDeceleration( void ) const { return decelTime; }
+	const type &		GetStartValue( void ) const { return startValue; }
+	const type &		GetEndValue( void ) const { return endValue; }
+
+private:
+	float				startTime;
+	float				accelTime;
+	float				linearTime;
+	float				decelTime;
+	type				startValue;
+	type				endValue;
+	mutable idExtrapolate<type> extrapolate;
+
+	void				Invalidate( void );
+	void				SetPhase( float time ) const;
+};
+
+/*
+====================
+idInterpolateAccelDecelLinear::idInterpolateAccelDecelLinear
+====================
+*/
+template< class type >
+ID_INLINE idInterpolateAccelDecelLinear<type>::idInterpolateAccelDecelLinear() {
+	startTime = accelTime = linearTime = decelTime = 0;
+	memset( &startValue, 0, sizeof( startValue ) );
+	endValue = startValue;
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::Init
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelLinear<type>::Init( const float startTime, const float accelTime, const float decelTime, const float duration, const type &startValue, const type &endValue ) {
+	type speed;
+
+	this->startTime = startTime;
+	this->accelTime = accelTime;
+	this->decelTime = decelTime;
+	this->startValue = startValue;
+	this->endValue = endValue;
+
+	if ( duration <= 0.0f ) {
+		return;
+	}
+
+	if ( this->accelTime + this->decelTime > duration ) {
+		this->accelTime = this->accelTime * duration / ( this->accelTime + this->decelTime );
+		this->decelTime = duration - this->accelTime;
+	}
+	this->linearTime = duration - this->accelTime - this->decelTime;
+	speed = ( endValue - startValue ) * ( 1000.0f / ( (float) this->linearTime + ( this->accelTime + this->decelTime ) * 0.5f ) );
+
+	if ( this->accelTime ) {
+		extrapolate.Init( startTime, this->accelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_ACCELLINEAR );
+	} else if ( this->linearTime ) {
+		extrapolate.Init( startTime, this->linearTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_LINEAR );
+	} else {
+		extrapolate.Init( startTime, this->decelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_DECELLINEAR );
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::Invalidate
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelLinear<type>::Invalidate( void ) {
+	extrapolate.Init( 0, 0, extrapolate.GetStartValue(), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_NONE );
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::SetPhase
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelLinear<type>::SetPhase( float time ) const {
+	float deltaTime;
+
+	deltaTime = time - startTime;
+	if ( deltaTime < accelTime ) {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_ACCELLINEAR ) {
+			extrapolate.Init( startTime, accelTime, startValue, extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_ACCELLINEAR );
+		}
+	} else if ( deltaTime < accelTime + linearTime ) {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_LINEAR ) {
+			extrapolate.Init( startTime + accelTime, linearTime, startValue + extrapolate.GetSpeed() * ( accelTime * 0.001f * 0.5f ), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_LINEAR );
+		}
+	} else {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_DECELLINEAR ) {
+			extrapolate.Init( startTime + accelTime + linearTime, decelTime, endValue - ( extrapolate.GetSpeed() * ( decelTime * 0.001f * 0.5f ) ), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_DECELLINEAR );
+		}
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelLinear<type>::GetCurrentValue( float time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentValue( time );
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelLinear<type>::GetCurrentSpeed( float time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentSpeed( time );
+}
+
+
+/*
+==============================================================================================
+
+	Continuous interpolation with sinusoidal acceleration and deceleration phase.
+	Both the velocity and acceleration are continuous.
+
+==============================================================================================
+*/
+
+template< class type >
+class idInterpolateAccelDecelSine  {
+public:
+						idInterpolateAccelDecelSine();
+
+	void				Init( const float startTime, const float accelTime, const float decelTime, const float duration, const type &startValue, const type &endValue );
+	void				SetStartTime( float time ) { startTime = time; Invalidate(); }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; Invalidate(); }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; Invalidate(); }
+
+	type				GetCurrentValue( float time ) const;
+	type				GetCurrentSpeed( float time ) const;
+	bool				IsDone( float time ) const { return ( time >= startTime + accelTime + linearTime + decelTime ); }
+
+	float				GetStartTime( void ) const { return startTime; }
+	float				GetEndTime( void ) const { return startTime + accelTime + linearTime + decelTime; }
+	float				GetDuration( void ) const { return accelTime + linearTime + decelTime; }
+	float				GetAcceleration( void ) const { return accelTime; }
+	float				GetDeceleration( void ) const { return decelTime; }
+	const type &		GetStartValue( void ) const { return startValue; }
+	const type &		GetEndValue( void ) const { return endValue; }
+
+private:
+	float				startTime;
+	float				accelTime;
+	float				linearTime;
+	float				decelTime;
+	type				startValue;
+	type				endValue;
+	mutable idExtrapolate<type> extrapolate;
+
+	void				Invalidate( void );
+	void				SetPhase( float time ) const;
+};
+
+/*
+====================
+idInterpolateAccelDecelSine::idInterpolateAccelDecelSine
+====================
+*/
+template< class type >
+ID_INLINE idInterpolateAccelDecelSine<type>::idInterpolateAccelDecelSine() {
+	startTime = accelTime = linearTime = decelTime = 0;
+	memset( &startValue, 0, sizeof( startValue ) );
+	endValue = startValue;
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::Init
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::Init( const float startTime, const float accelTime, const float decelTime, const float duration, const type &startValue, const type &endValue ) {
+	type speed;
+
+	this->startTime = startTime;
+	this->accelTime = accelTime;
+	this->decelTime = decelTime;
+	this->startValue = startValue;
+	this->endValue = endValue;
+
+	if ( duration <= 0.0f ) {
+		return;
+	}
+
+	if ( this->accelTime + this->decelTime > duration ) {
+		this->accelTime = this->accelTime * duration / ( this->accelTime + this->decelTime );
+		this->decelTime = duration - this->accelTime;
+	}
+	this->linearTime = duration - this->accelTime - this->decelTime;
+	speed = ( endValue - startValue ) * ( 1000.0f / ( (float) this->linearTime + ( this->accelTime + this->decelTime ) * idMath::SQRT_1OVER2 ) );
+
+	if ( this->accelTime ) {
+		extrapolate.Init( startTime, this->accelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_ACCELSINE );
+	} else if ( this->linearTime ) {
+		extrapolate.Init( startTime, this->linearTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_LINEAR );
+	} else {
+		extrapolate.Init( startTime, this->decelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_DECELSINE );
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::Invalidate
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::Invalidate( void ) {
+	extrapolate.Init( 0, 0, extrapolate.GetStartValue(), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_NONE );
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::SetPhase
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::SetPhase( float time ) const {
+	float deltaTime;
+
+	deltaTime = time - startTime;
+	if ( deltaTime < accelTime ) {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_ACCELSINE ) {
+			extrapolate.Init( startTime, accelTime, startValue, extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_ACCELSINE );
+		}
+	} else if ( deltaTime < accelTime + linearTime ) {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_LINEAR ) {
+			extrapolate.Init( startTime + accelTime, linearTime, startValue + extrapolate.GetSpeed() * ( accelTime * 0.001f * idMath::SQRT_1OVER2 ), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_LINEAR );
+		}
+	} else {
+		if ( extrapolate.GetExtrapolationType() != EXTRAPOLATION_DECELSINE ) {
+			extrapolate.Init( startTime + accelTime + linearTime, decelTime, endValue - ( extrapolate.GetSpeed() * ( decelTime * 0.001f * idMath::SQRT_1OVER2 ) ), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_DECELSINE );
+		}
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelSine<type>::GetCurrentValue( float time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentValue( time );
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelSine<type>::GetCurrentSpeed( float time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentSpeed( time );
+}
+
+#endif /* !__MATH_INTERPOLATE_H__ */

neo/idlib/math/Lcp.h

@@ -0,0 +1,77 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_LCP_H__
+#define __MATH_LCP_H__
+
+/*
+===============================================================================
+
+  Box Constrained Mixed Linear Complementarity Problem solver
+
+  A is a matrix of dimension n*n and x, b, lo, hi are vectors of dimension n
+
+  Solve: Ax = b + t, where t is a vector of dimension n, with
+  complementarity condition: (x[i] - lo[i]) * (x[i] - hi[i]) * t[i] = 0
+  such that for each 0 <= i < n one of the following holds:
+
+    1. lo[i] < x[i] < hi[i], t[i] == 0
+    2. x[i] == lo[i], t[i] >= 0
+    3. x[i] == hi[i], t[i] <= 0
+
+  Partly bounded or unbounded variables can have lo[i] and/or hi[i]
+  set to negative/positive idMath::INFITITY respectively.
+
+  If boxIndex != NULL and boxIndex[i] != -1 then
+
+    lo[i] = - fabs( lo[i] * x[boxIndex[i]] )
+    hi[i] = fabs( hi[i] * x[boxIndex[i]] )
+	boxIndex[boxIndex[i]] must be -1
+  
+  Before calculating any of the bounded x[i] with boxIndex[i] != -1 the
+  solver calculates all unbounded x[i] and all x[i] with boxIndex[i] == -1.
+
+===============================================================================
+*/
+
+class idLCP {
+public:
+	static idLCP *	AllocSquare( void );		// A must be a square matrix
+	static idLCP *	AllocSymmetric( void );		// A must be a symmetric matrix
+
+	virtual			~idLCP( void );
+
+	virtual bool	Solve( const idMatX &A, idVecX &x, const idVecX &b, const idVecX &lo, const idVecX &hi, const int *boxIndex = NULL ) = 0;
+	virtual void	SetMaxIterations( int max );
+	virtual int		GetMaxIterations( void );
+
+protected:
+	int				maxIterations;
+};
+
+#endif /* !__MATH_LCP_H__ */

neo/idlib/math/Math.cpp

@@ -0,0 +1,131 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+
+const float	idMath::PI				= 3.14159265358979323846f;
+const float	idMath::TWO_PI			= 2.0f * PI;
+const float	idMath::HALF_PI			= 0.5f * PI;
+const float	idMath::ONEFOURTH_PI	= 0.25f * PI;
+const float idMath::E				= 2.71828182845904523536f;
+const float idMath::SQRT_TWO		= 1.41421356237309504880f;
+const float idMath::SQRT_THREE		= 1.73205080756887729352f;
+const float	idMath::SQRT_1OVER2		= 0.70710678118654752440f;
+const float	idMath::SQRT_1OVER3		= 0.57735026918962576450f;
+const float	idMath::M_DEG2RAD		= PI / 180.0f;
+const float	idMath::M_RAD2DEG		= 180.0f / PI;
+const float	idMath::M_SEC2MS		= 1000.0f;
+const float	idMath::M_MS2SEC		= 0.001f;
+const float	idMath::INFINITY		= 1e30f;
+const float idMath::FLT_EPSILON		= 1.192092896e-07f;
+
+bool		idMath::initialized		= false;
+dword		idMath::iSqrt[SQRT_TABLE_SIZE];		// inverse square root lookup table
+
+/*
+===============
+idMath::Init
+===============
+*/
+void idMath::Init( void ) {
+    union _flint fi, fo;
+
+    for ( int i = 0; i < SQRT_TABLE_SIZE; i++ ) {
+        fi.i	 = ((EXP_BIAS-1) << EXP_POS) | (i << LOOKUP_POS);
+        fo.f	 = (float)( 1.0 / sqrt( fi.f ) );
+        iSqrt[i] = ((dword)(((fo.i + (1<<(SEED_POS-2))) >> SEED_POS) & 0xFF))<<SEED_POS;
+    }
+    
+	iSqrt[SQRT_TABLE_SIZE / 2] = ((dword)(0xFF))<<(SEED_POS); 
+
+	initialized = true;
+}
+
+/*
+================
+idMath::FloatToBits
+================
+*/
+int idMath::FloatToBits( float f, int exponentBits, int mantissaBits ) {
+	int i, sign, exponent, mantissa, value;
+
+	assert( exponentBits >= 2 && exponentBits <= 8 );
+	assert( mantissaBits >= 2 && mantissaBits <= 23 );
+
+	int maxBits = ( ( ( 1 << ( exponentBits - 1 ) ) - 1 ) << mantissaBits ) | ( ( 1 << mantissaBits ) - 1 );
+	int minBits = ( ( ( 1 <<   exponentBits       ) - 2 ) << mantissaBits ) | 1;
+
+	float max = BitsToFloat( maxBits, exponentBits, mantissaBits );
+	float min = BitsToFloat( minBits, exponentBits, mantissaBits );
+
+	if ( f >= 0.0f ) {
+		if ( f >= max ) {
+			return maxBits;
+		} else if ( f <= min ) {
+			return minBits;
+		}
+	} else {
+		if ( f <= -max ) {
+			return ( maxBits | ( 1 << ( exponentBits + mantissaBits ) ) );
+		} else if ( f >= -min ) {
+			return ( minBits | ( 1 << ( exponentBits + mantissaBits ) ) );
+		}
+	}
+
+	exponentBits--;
+	i = *reinterpret_cast<int *>(&f);
+	sign = ( i >> IEEE_FLT_SIGN_BIT ) & 1;
+	exponent = ( ( i >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+	mantissa = i & ( ( 1 << IEEE_FLT_MANTISSA_BITS ) - 1 );
+	value = sign << ( 1 + exponentBits + mantissaBits );
+	value |= ( ( INTSIGNBITSET( exponent ) << exponentBits ) | ( abs( exponent ) & ( ( 1 << exponentBits ) - 1 ) ) ) << mantissaBits;
+	value |= mantissa >> ( IEEE_FLT_MANTISSA_BITS - mantissaBits );
+	return value;
+}
+
+/*
+================
+idMath::BitsToFloat
+================
+*/
+float idMath::BitsToFloat( int i, int exponentBits, int mantissaBits ) {
+	static int exponentSign[2] = { 1, -1 };
+	int sign, exponent, mantissa, value;
+
+	assert( exponentBits >= 2 && exponentBits <= 8 );
+	assert( mantissaBits >= 2 && mantissaBits <= 23 );
+
+	exponentBits--;
+	sign = i >> ( 1 + exponentBits + mantissaBits );
+	exponent = ( ( i >> mantissaBits ) & ( ( 1 << exponentBits ) - 1 ) ) * exponentSign[( i >> ( exponentBits + mantissaBits ) ) & 1];
+	mantissa = ( i & ( ( 1 << mantissaBits ) - 1 ) ) << ( IEEE_FLT_MANTISSA_BITS - mantissaBits );
+	value = sign << IEEE_FLT_SIGN_BIT | ( exponent + IEEE_FLT_EXPONENT_BIAS ) << IEEE_FLT_MANTISSA_BITS | mantissa;
+	return *reinterpret_cast<float *>(&value);
+}

neo/idlib/math/Math.h

@@ -0,0 +1,931 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_MATH_H__
+#define __MATH_MATH_H__
+
+#ifdef MACOS_X
+// for square root estimate instruction
+#include <ppc_intrinsics.h>
+// for FLT_MIN
+#include <float.h>
+#endif
+/*
+===============================================================================
+
+  Math
+
+===============================================================================
+*/
+
+#ifdef INFINITY
+#undef INFINITY
+#endif
+
+#ifdef FLT_EPSILON
+#undef FLT_EPSILON
+#endif
+
+#define DEG2RAD(a)				( (a) * idMath::M_DEG2RAD )
+#define RAD2DEG(a)				( (a) * idMath::M_RAD2DEG )
+
+#define SEC2MS(t)				( idMath::FtoiFast( (t) * idMath::M_SEC2MS ) )
+#define MS2SEC(t)				( (t) * idMath::M_MS2SEC )
+
+#define	ANGLE2SHORT(x)			( idMath::FtoiFast( (x) * 65536.0f / 360.0f ) & 65535 )
+#define	SHORT2ANGLE(x)			( (x) * ( 360.0f / 65536.0f ) )
+
+#define	ANGLE2BYTE(x)			( idMath::FtoiFast( (x) * 256.0f / 360.0f ) & 255 )
+#define	BYTE2ANGLE(x)			( (x) * ( 360.0f / 256.0f ) )
+
+#define FLOATSIGNBITSET(f)		((*(const unsigned long *)&(f)) >> 31)
+#define FLOATSIGNBITNOTSET(f)	((~(*(const unsigned long *)&(f))) >> 31)
+#define FLOATNOTZERO(f)			((*(const unsigned long *)&(f)) & ~(1<<31) )
+#define INTSIGNBITSET(i)		(((const unsigned long)(i)) >> 31)
+#define INTSIGNBITNOTSET(i)		((~((const unsigned long)(i))) >> 31)
+
+#define	FLOAT_IS_NAN(x)			(((*(const unsigned long *)&x) & 0x7f800000) == 0x7f800000)
+#define FLOAT_IS_INF(x)			(((*(const unsigned long *)&x) & 0x7fffffff) == 0x7f800000)
+#define FLOAT_IS_IND(x)			((*(const unsigned long *)&x) == 0xffc00000)
+#define	FLOAT_IS_DENORMAL(x)	(((*(const unsigned long *)&x) & 0x7f800000) == 0x00000000 && \
+								 ((*(const unsigned long *)&x) & 0x007fffff) != 0x00000000 )
+
+#define IEEE_FLT_MANTISSA_BITS	23
+#define IEEE_FLT_EXPONENT_BITS	8
+#define IEEE_FLT_EXPONENT_BIAS	127
+#define IEEE_FLT_SIGN_BIT		31
+
+#define IEEE_DBL_MANTISSA_BITS	52
+#define IEEE_DBL_EXPONENT_BITS	11
+#define IEEE_DBL_EXPONENT_BIAS	1023
+#define IEEE_DBL_SIGN_BIT		63
+
+#define IEEE_DBLE_MANTISSA_BITS	63
+#define IEEE_DBLE_EXPONENT_BITS	15
+#define IEEE_DBLE_EXPONENT_BIAS	0
+#define IEEE_DBLE_SIGN_BIT		79
+
+template<class T> ID_INLINE int	MaxIndex( T x, T y ) { return  ( x > y ) ? 0 : 1; }
+template<class T> ID_INLINE int	MinIndex( T x, T y ) { return ( x < y ) ? 0 : 1; }
+
+template<class T> ID_INLINE T	Max3( T x, T y, T z ) { return ( x > y ) ? ( ( x > z ) ? x : z ) : ( ( y > z ) ? y : z ); }
+template<class T> ID_INLINE T	Min3( T x, T y, T z ) { return ( x < y ) ? ( ( x < z ) ? x : z ) : ( ( y < z ) ? y : z ); }
+template<class T> ID_INLINE int	Max3Index( T x, T y, T z ) { return ( x > y ) ? ( ( x > z ) ? 0 : 2 ) : ( ( y > z ) ? 1 : 2 ); }
+template<class T> ID_INLINE int	Min3Index( T x, T y, T z ) { return ( x < y ) ? ( ( x < z ) ? 0 : 2 ) : ( ( y < z ) ? 1 : 2 ); }
+
+template<class T> ID_INLINE T	Sign( T f ) { return ( f > 0 ) ? 1 : ( ( f < 0 ) ? -1 : 0 ); }
+template<class T> ID_INLINE T	Square( T x ) { return x * x; }
+template<class T> ID_INLINE T	Cube( T x ) { return x * x * x; }
+
+
+class idMath {
+public:
+
+	static void					Init( void );
+
+	static float				RSqrt( float x );			// reciprocal square root, returns huge number when x == 0.0
+
+	static float				InvSqrt( float x );			// inverse square root with 32 bits precision, returns huge number when x == 0.0
+	static float				InvSqrt16( float x );		// inverse square root with 16 bits precision, returns huge number when x == 0.0
+	static double				InvSqrt64( float x );		// inverse square root with 64 bits precision, returns huge number when x == 0.0
+
+	static float				Sqrt( float x );			// square root with 32 bits precision
+	static float				Sqrt16( float x );			// square root with 16 bits precision
+	static double				Sqrt64( float x );			// square root with 64 bits precision
+
+	static float				Sin( float a );				// sine with 32 bits precision
+	static float				Sin16( float a );			// sine with 16 bits precision, maximum absolute error is 2.3082e-09
+	static double				Sin64( float a );			// sine with 64 bits precision
+
+	static float				Cos( float a );				// cosine with 32 bits precision
+	static float				Cos16( float a );			// cosine with 16 bits precision, maximum absolute error is 2.3082e-09
+	static double				Cos64( float a );			// cosine with 64 bits precision
+
+	static void					SinCos( float a, float &s, float &c );		// sine and cosine with 32 bits precision
+	static void					SinCos16( float a, float &s, float &c );	// sine and cosine with 16 bits precision
+	static void					SinCos64( float a, double &s, double &c );	// sine and cosine with 64 bits precision
+
+	static float				Tan( float a );				// tangent with 32 bits precision
+	static float				Tan16( float a );			// tangent with 16 bits precision, maximum absolute error is 1.8897e-08
+	static double				Tan64( float a );			// tangent with 64 bits precision
+
+	static float				ASin( float a );			// arc sine with 32 bits precision, input is clamped to [-1, 1] to avoid a silent NaN
+	static float				ASin16( float a );			// arc sine with 16 bits precision, maximum absolute error is 6.7626e-05
+	static double				ASin64( float a );			// arc sine with 64 bits precision
+
+	static float				ACos( float a );			// arc cosine with 32 bits precision, input is clamped to [-1, 1] to avoid a silent NaN
+	static float				ACos16( float a );			// arc cosine with 16 bits precision, maximum absolute error is 6.7626e-05
+	static double				ACos64( float a );			// arc cosine with 64 bits precision
+
+	static float				ATan( float a );			// arc tangent with 32 bits precision
+	static float				ATan16( float a );			// arc tangent with 16 bits precision, maximum absolute error is 1.3593e-08
+	static double				ATan64( float a );			// arc tangent with 64 bits precision
+
+	static float				ATan( float y, float x );	// arc tangent with 32 bits precision
+	static float				ATan16( float y, float x );	// arc tangent with 16 bits precision, maximum absolute error is 1.3593e-08
+	static double				ATan64( float y, float x );	// arc tangent with 64 bits precision
+
+	static float				Pow( float x, float y );	// x raised to the power y with 32 bits precision
+	static float				Pow16( float x, float y );	// x raised to the power y with 16 bits precision
+	static double				Pow64( float x, float y );	// x raised to the power y with 64 bits precision
+
+	static float				Exp( float f );				// e raised to the power f with 32 bits precision
+	static float				Exp16( float f );			// e raised to the power f with 16 bits precision
+	static double				Exp64( float f );			// e raised to the power f with 64 bits precision
+
+	static float				Log( float f );				// natural logarithm with 32 bits precision
+	static float				Log16( float f );			// natural logarithm with 16 bits precision
+	static double				Log64( float f );			// natural logarithm with 64 bits precision
+
+	static int					IPow( int x, int y );		// integral x raised to the power y
+	static int					ILog2( float f );			// integral base-2 logarithm of the floating point value
+	static int					ILog2( int i );				// integral base-2 logarithm of the integer value
+
+	static int					BitsForFloat( float f );	// minumum number of bits required to represent ceil( f )
+	static int					BitsForInteger( int i );	// minumum number of bits required to represent i
+	static int					MaskForFloatSign( float f );// returns 0x00000000 if x >= 0.0f and returns 0xFFFFFFFF if x <= -0.0f
+	static int					MaskForIntegerSign( int i );// returns 0x00000000 if x >= 0 and returns 0xFFFFFFFF if x < 0
+	static int					FloorPowerOfTwo( int x );	// round x down to the nearest power of 2
+	static int					CeilPowerOfTwo( int x );	// round x up to the nearest power of 2
+	static bool					IsPowerOfTwo( int x );		// returns true if x is a power of 2
+	static int					BitCount( int x );			// returns the number of 1 bits in x
+	static int					BitReverse( int x );		// returns the bit reverse of x
+
+	static int					Abs( int x );				// returns the absolute value of the integer value (for reference only)
+	static float				Fabs( float f );			// returns the absolute value of the floating point value
+	static float				Floor( float f );			// returns the largest integer that is less than or equal to the given value
+	static float				Ceil( float f );			// returns the smallest integer that is greater than or equal to the given value
+	static float				Rint( float f );			// returns the nearest integer
+	static int					Ftoi( float f );			// float to int conversion
+	static int					FtoiFast( float f );		// fast float to int conversion but uses current FPU round mode (default round nearest)
+	static unsigned long		Ftol( float f );			// float to long conversion
+	static unsigned long		FtolFast( float );			// fast float to long conversion but uses current FPU round mode (default round nearest)
+
+	static signed char			ClampChar( int i );
+	static signed short			ClampShort( int i );
+	static int					ClampInt( int min, int max, int value );
+	static float				ClampFloat( float min, float max, float value );
+
+	static float				AngleNormalize360( float angle );
+	static float				AngleNormalize180( float angle );
+	static float				AngleDelta( float angle1, float angle2 );
+
+	static int					FloatToBits( float f, int exponentBits, int mantissaBits );
+	static float				BitsToFloat( int i, int exponentBits, int mantissaBits );
+
+	static int					FloatHash( const float *array, const int numFloats );
+
+	static const float			PI;							// pi
+	static const float			TWO_PI;						// pi * 2
+	static const float			HALF_PI;					// pi / 2
+	static const float			ONEFOURTH_PI;				// pi / 4
+	static const float			E;							// e
+	static const float			SQRT_TWO;					// sqrt( 2 )
+	static const float			SQRT_THREE;					// sqrt( 3 )
+	static const float			SQRT_1OVER2;				// sqrt( 1 / 2 )
+	static const float			SQRT_1OVER3;				// sqrt( 1 / 3 )
+	static const float			M_DEG2RAD;					// degrees to radians multiplier
+	static const float			M_RAD2DEG;					// radians to degrees multiplier
+	static const float			M_SEC2MS;					// seconds to milliseconds multiplier
+	static const float			M_MS2SEC;					// milliseconds to seconds multiplier
+	static const float			INFINITY;					// huge number which should be larger than any valid number used
+	static const float			FLT_EPSILON;				// smallest positive number such that 1.0+FLT_EPSILON != 1.0
+
+private:
+	enum {
+		LOOKUP_BITS				= 8,							
+		EXP_POS					= 23,							
+		EXP_BIAS				= 127,							
+		LOOKUP_POS				= (EXP_POS-LOOKUP_BITS),
+		SEED_POS				= (EXP_POS-8),
+		SQRT_TABLE_SIZE			= (2<<LOOKUP_BITS),
+		LOOKUP_MASK				= (SQRT_TABLE_SIZE-1)
+	};
+
+	union _flint {
+		dword					i;
+		float					f;
+	};
+
+	static dword				iSqrt[SQRT_TABLE_SIZE];
+	static bool					initialized;
+};
+
+ID_INLINE float idMath::RSqrt( float x ) {
+
+	long i;
+	float y, r;
+
+	y = x * 0.5f;
+	i = *reinterpret_cast<long *>( &x );
+	i = 0x5f3759df - ( i >> 1 );
+	r = *reinterpret_cast<float *>( &i );
+	r = r * ( 1.5f - r * r * y );
+	return r;
+}
+
+ID_INLINE float idMath::InvSqrt16( float x ) {
+
+	dword a = ((union _flint*)(&x))->i;
+	union _flint seed;
+
+	assert( initialized );
+
+	double y = x * 0.5f;
+	seed.i = (( ( (3*EXP_BIAS-1) - ( (a >> EXP_POS) & 0xFF) ) >> 1)<<EXP_POS) | iSqrt[(a >> (EXP_POS-LOOKUP_BITS)) & LOOKUP_MASK];
+	double r = seed.f;
+	r = r * ( 1.5f - r * r * y );
+	return (float) r;
+}
+
+ID_INLINE float idMath::InvSqrt( float x ) {
+
+	dword a = ((union _flint*)(&x))->i;
+	union _flint seed;
+
+	assert( initialized );
+
+	double y = x * 0.5f;
+	seed.i = (( ( (3*EXP_BIAS-1) - ( (a >> EXP_POS) & 0xFF) ) >> 1)<<EXP_POS) | iSqrt[(a >> (EXP_POS-LOOKUP_BITS)) & LOOKUP_MASK];
+	double r = seed.f;
+	r = r * ( 1.5f - r * r * y );
+	r = r * ( 1.5f - r * r * y );
+	return (float) r;
+}
+
+ID_INLINE double idMath::InvSqrt64( float x ) {
+	dword a = ((union _flint*)(&x))->i;
+	union _flint seed;
+
+	assert( initialized );
+
+	double y = x * 0.5f;
+	seed.i = (( ( (3*EXP_BIAS-1) - ( (a >> EXP_POS) & 0xFF) ) >> 1)<<EXP_POS) | iSqrt[(a >> (EXP_POS-LOOKUP_BITS)) & LOOKUP_MASK];
+	double r = seed.f;
+	r = r * ( 1.5f - r * r * y );
+	r = r * ( 1.5f - r * r * y );
+	r = r * ( 1.5f - r * r * y );
+	return r;
+}
+
+ID_INLINE float idMath::Sqrt16( float x ) {
+	return x * InvSqrt16( x );
+}
+
+ID_INLINE float idMath::Sqrt( float x ) {
+	return x * InvSqrt( x );
+}
+
+ID_INLINE double idMath::Sqrt64( float x ) {
+	return x * InvSqrt64( x );
+}
+
+ID_INLINE float idMath::Sin( float a ) {
+	return sinf( a );
+}
+
+ID_INLINE float idMath::Sin16( float a ) {
+	float s;
+
+	if ( ( a < 0.0f ) || ( a >= TWO_PI ) ) {
+		a -= floorf( a / TWO_PI ) * TWO_PI;
+	}
+#if 1
+	if ( a < PI ) {
+		if ( a > HALF_PI ) {
+			a = PI - a;
+		}
+	} else {
+		if ( a > PI + HALF_PI ) {
+			a = a - TWO_PI;
+		} else {
+			a = PI - a;
+		}
+	}
+#else
+	a = PI - a;
+	if ( fabs( a ) >= HALF_PI ) {
+		a = ( ( a < 0.0f ) ? -PI : PI ) - a;
+	}
+#endif
+	s = a * a;
+	return a * ( ( ( ( ( -2.39e-08f * s + 2.7526e-06f ) * s - 1.98409e-04f ) * s + 8.3333315e-03f ) * s - 1.666666664e-01f ) * s + 1.0f );
+}
+
+ID_INLINE double idMath::Sin64( float a ) {
+	return sin( a );
+}
+
+ID_INLINE float idMath::Cos( float a ) {
+	return cosf( a );
+}
+
+ID_INLINE float idMath::Cos16( float a ) {
+	float s, d;
+
+	if ( ( a < 0.0f ) || ( a >= TWO_PI ) ) {
+		a -= floorf( a / TWO_PI ) * TWO_PI;
+	}
+#if 1
+	if ( a < PI ) {
+		if ( a > HALF_PI ) {
+			a = PI - a;
+			d = -1.0f;
+		} else {
+			d = 1.0f;
+		}
+	} else {
+		if ( a > PI + HALF_PI ) {
+			a = a - TWO_PI;
+			d = 1.0f;
+		} else {
+			a = PI - a;
+			d = -1.0f;
+		}
+	}
+#else
+	a = PI - a;
+	if ( fabs( a ) >= HALF_PI ) {
+		a = ( ( a < 0.0f ) ? -PI : PI ) - a;
+		d = 1.0f;
+	} else {
+		d = -1.0f;
+	}
+#endif
+	s = a * a;
+	return d * ( ( ( ( ( -2.605e-07f * s + 2.47609e-05f ) * s - 1.3888397e-03f ) * s + 4.16666418e-02f ) * s - 4.999999963e-01f ) * s + 1.0f );
+}
+
+ID_INLINE double idMath::Cos64( float a ) {
+	return cos( a );
+}
+
+ID_INLINE void idMath::SinCos( float a, float &s, float &c ) {
+#ifdef _WIN32
+	_asm {
+		fld		a
+		fsincos
+		mov		ecx, c
+		mov		edx, s
+		fstp	dword ptr [ecx]
+		fstp	dword ptr [edx]
+	}
+#else
+	s = sinf( a );
+	c = cosf( a );
+#endif
+}
+
+ID_INLINE void idMath::SinCos16( float a, float &s, float &c ) {
+	float t, d;
+
+	if ( ( a < 0.0f ) || ( a >= idMath::TWO_PI ) ) {
+		a -= floorf( a / idMath::TWO_PI ) * idMath::TWO_PI;
+	}
+#if 1
+	if ( a < PI ) {
+		if ( a > HALF_PI ) {
+			a = PI - a;
+			d = -1.0f;
+		} else {
+			d = 1.0f;
+		}
+	} else {
+		if ( a > PI + HALF_PI ) {
+			a = a - TWO_PI;
+			d = 1.0f;
+		} else {
+			a = PI - a;
+			d = -1.0f;
+		}
+	}
+#else
+	a = PI - a;
+	if ( fabs( a ) >= HALF_PI ) {
+		a = ( ( a < 0.0f ) ? -PI : PI ) - a;
+		d = 1.0f;
+	} else {
+		d = -1.0f;
+	}
+#endif
+	t = a * a;
+	s = a * ( ( ( ( ( -2.39e-08f * t + 2.7526e-06f ) * t - 1.98409e-04f ) * t + 8.3333315e-03f ) * t - 1.666666664e-01f ) * t + 1.0f );
+	c = d * ( ( ( ( ( -2.605e-07f * t + 2.47609e-05f ) * t - 1.3888397e-03f ) * t + 4.16666418e-02f ) * t - 4.999999963e-01f ) * t + 1.0f );
+}
+
+ID_INLINE void idMath::SinCos64( float a, double &s, double &c ) {
+#ifdef _WIN32
+	_asm {
+		fld		a
+		fsincos
+		mov		ecx, c
+		mov		edx, s
+		fstp	qword ptr [ecx]
+		fstp	qword ptr [edx]
+	}
+#else
+	s = sin( a );
+	c = cos( a );
+#endif
+}
+
+ID_INLINE float idMath::Tan( float a ) {
+	return tanf( a );
+}
+
+ID_INLINE float idMath::Tan16( float a ) {
+	float s;
+	bool reciprocal;
+
+	if ( ( a < 0.0f ) || ( a >= PI ) ) {
+		a -= floorf( a / PI ) * PI;
+	}
+#if 1
+	if ( a < HALF_PI ) {
+		if ( a > ONEFOURTH_PI ) {
+			a = HALF_PI - a;
+			reciprocal = true;
+		} else {
+			reciprocal = false;
+		}
+	} else {
+		if ( a > HALF_PI + ONEFOURTH_PI ) {
+			a = a - PI;
+			reciprocal = false;
+		} else {
+			a = HALF_PI - a;
+			reciprocal = true;
+		}
+	}
+#else
+	a = HALF_PI - a;
+	if ( fabs( a ) >= ONEFOURTH_PI ) {
+		a = ( ( a < 0.0f ) ? -HALF_PI : HALF_PI ) - a;
+		reciprocal = false;
+	} else {
+		reciprocal = true;
+	}
+#endif
+	s = a * a;
+	s = a * ( ( ( ( ( ( 9.5168091e-03f * s + 2.900525e-03f ) * s + 2.45650893e-02f ) * s + 5.33740603e-02f ) * s + 1.333923995e-01f ) * s + 3.333314036e-01f ) * s + 1.0f );
+	if ( reciprocal ) {
+		return 1.0f / s;
+	} else {
+		return s;
+	}
+}
+
+ID_INLINE double idMath::Tan64( float a ) {
+	return tan( a );
+}
+
+ID_INLINE float idMath::ASin( float a ) {
+	if ( a <= -1.0f ) {
+		return -HALF_PI;
+	}
+	if ( a >= 1.0f ) {
+		return HALF_PI;
+	}
+	return asinf( a );
+}
+
+ID_INLINE float idMath::ASin16( float a ) {
+	if ( FLOATSIGNBITSET( a ) ) {
+		if ( a <= -1.0f ) {
+			return -HALF_PI;
+		}
+		a = fabs( a );
+		return ( ( ( -0.0187293f * a + 0.0742610f ) * a - 0.2121144f ) * a + 1.5707288f ) * sqrt( 1.0f - a ) - HALF_PI;
+	} else {
+		if ( a >= 1.0f ) {
+			return HALF_PI;
+		}
+		return HALF_PI - ( ( ( -0.0187293f * a + 0.0742610f ) * a - 0.2121144f ) * a + 1.5707288f ) * sqrt( 1.0f - a );
+	}
+}
+
+ID_INLINE double idMath::ASin64( float a ) {
+	if ( a <= -1.0f ) {
+		return -HALF_PI;
+	}
+	if ( a >= 1.0f ) {
+		return HALF_PI;
+	}
+	return asin( a );
+}
+
+ID_INLINE float idMath::ACos( float a ) {
+	if ( a <= -1.0f ) {
+		return PI;
+	}
+	if ( a >= 1.0f ) {
+		return 0.0f;
+	}
+	return acosf( a );
+}
+
+ID_INLINE float idMath::ACos16( float a ) {
+	if ( FLOATSIGNBITSET( a ) ) {
+		if ( a <= -1.0f ) {
+			return PI;
+		}
+		a = fabs( a );
+		return PI - ( ( ( -0.0187293f * a + 0.0742610f ) * a - 0.2121144f ) * a + 1.5707288f ) * sqrt( 1.0f - a );
+	} else {
+		if ( a >= 1.0f ) {
+			return 0.0f;
+		}
+		return ( ( ( -0.0187293f * a + 0.0742610f ) * a - 0.2121144f ) * a + 1.5707288f ) * sqrt( 1.0f - a );
+	}
+}
+
+ID_INLINE double idMath::ACos64( float a ) {
+	if ( a <= -1.0f ) {
+		return PI;
+	}
+	if ( a >= 1.0f ) {
+		return 0.0f;
+	}
+	return acos( a );
+}
+
+ID_INLINE float idMath::ATan( float a ) {
+	return atanf( a );
+}
+
+ID_INLINE float idMath::ATan16( float a ) {
+	float s;
+
+	if ( fabs( a ) > 1.0f ) {
+		a = 1.0f / a;
+		s = a * a;
+		s = - ( ( ( ( ( ( ( ( ( 0.0028662257f * s - 0.0161657367f ) * s + 0.0429096138f ) * s - 0.0752896400f )
+				* s + 0.1065626393f ) * s - 0.1420889944f ) * s + 0.1999355085f ) * s - 0.3333314528f ) * s ) + 1.0f ) * a;
+		if ( FLOATSIGNBITSET( a ) ) {
+			return s - HALF_PI;
+		} else {
+			return s + HALF_PI;
+		}
+	} else {
+		s = a * a;
+		return ( ( ( ( ( ( ( ( ( 0.0028662257f * s - 0.0161657367f ) * s + 0.0429096138f ) * s - 0.0752896400f )
+			* s + 0.1065626393f ) * s - 0.1420889944f ) * s + 0.1999355085f ) * s - 0.3333314528f ) * s ) + 1.0f ) * a;
+	}
+}
+
+ID_INLINE double idMath::ATan64( float a ) {
+	return atan( a );
+}
+
+ID_INLINE float idMath::ATan( float y, float x ) {
+	return atan2f( y, x );
+}
+
+ID_INLINE float idMath::ATan16( float y, float x ) {
+	float a, s;
+
+	if ( fabs( y ) > fabs( x ) ) {
+		a = x / y;
+		s = a * a;
+		s = - ( ( ( ( ( ( ( ( ( 0.0028662257f * s - 0.0161657367f ) * s + 0.0429096138f ) * s - 0.0752896400f )
+				* s + 0.1065626393f ) * s - 0.1420889944f ) * s + 0.1999355085f ) * s - 0.3333314528f ) * s ) + 1.0f ) * a;
+		if ( FLOATSIGNBITSET( a ) ) {
+			return s - HALF_PI;
+		} else {
+			return s + HALF_PI;
+		}
+	} else {
+		a = y / x;
+		s = a * a;
+		return ( ( ( ( ( ( ( ( ( 0.0028662257f * s - 0.0161657367f ) * s + 0.0429096138f ) * s - 0.0752896400f )
+			* s + 0.1065626393f ) * s - 0.1420889944f ) * s + 0.1999355085f ) * s - 0.3333314528f ) * s ) + 1.0f ) * a;
+	}
+}
+
+ID_INLINE double idMath::ATan64( float y, float x ) {
+	return atan2( y, x );
+}
+
+ID_INLINE float idMath::Pow( float x, float y ) {
+	return powf( x, y );
+}
+
+ID_INLINE float idMath::Pow16( float x, float y ) {
+	return Exp16( y * Log16( x ) );
+}
+
+ID_INLINE double idMath::Pow64( float x, float y ) {
+	return pow( x, y );
+}
+
+ID_INLINE float idMath::Exp( float f ) {
+	return expf( f );
+}
+
+ID_INLINE float idMath::Exp16( float f ) {
+	int i, s, e, m, exponent;
+	float x, x2, y, p, q;
+
+	x = f * 1.44269504088896340f;		// multiply with ( 1 / log( 2 ) )
+#if 1
+	i = *reinterpret_cast<int *>(&x);
+	s = ( i >> IEEE_FLT_SIGN_BIT );
+	e = ( ( i >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+	m = ( i & ( ( 1 << IEEE_FLT_MANTISSA_BITS ) - 1 ) ) | ( 1 << IEEE_FLT_MANTISSA_BITS );
+	i = ( ( m >> ( IEEE_FLT_MANTISSA_BITS - e ) ) & ~( e >> 31 ) ) ^ s;
+#else
+	i = (int) x;
+	if ( x < 0.0f ) {
+		i--;
+	}
+#endif
+	exponent = ( i + IEEE_FLT_EXPONENT_BIAS ) << IEEE_FLT_MANTISSA_BITS;
+	y = *reinterpret_cast<float *>(&exponent);
+	x -= (float) i;
+	if ( x >= 0.5f ) {
+		x -= 0.5f;
+		y *= 1.4142135623730950488f;	// multiply with sqrt( 2 )
+	}
+	x2 = x * x;
+	p = x * ( 7.2152891511493f + x2 * 0.0576900723731f );
+	q = 20.8189237930062f + x2;
+	x = y * ( q + p ) / ( q - p );
+	return x;
+}
+
+ID_INLINE double idMath::Exp64( float f ) {
+	return exp( f );
+}
+
+ID_INLINE float idMath::Log( float f ) {
+	return logf( f );
+}
+
+ID_INLINE float idMath::Log16( float f ) {
+	int i, exponent;
+	float y, y2;
+
+	i = *reinterpret_cast<int *>(&f);
+	exponent = ( ( i >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+	i -= ( exponent + 1 ) << IEEE_FLT_MANTISSA_BITS;	// get value in the range [.5, 1>
+	y = *reinterpret_cast<float *>(&i);
+	y *= 1.4142135623730950488f;						// multiply with sqrt( 2 )
+	y = ( y - 1.0f ) / ( y + 1.0f );
+	y2 = y * y;
+	y = y * ( 2.000000000046727f + y2 * ( 0.666666635059382f + y2 * ( 0.4000059794795f + y2 * ( 0.28525381498f + y2 * 0.2376245609f ) ) ) );
+	y += 0.693147180559945f * ( (float)exponent + 0.5f );
+	return y;
+}
+
+ID_INLINE double idMath::Log64( float f ) {
+	return log( f );
+}
+
+ID_INLINE int idMath::IPow( int x, int y ) {
+	int r; for( r = x; y > 1; y-- ) { r *= x; } return r;
+}
+
+ID_INLINE int idMath::ILog2( float f ) {
+	return ( ( (*reinterpret_cast<int *>(&f)) >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+}
+
+ID_INLINE int idMath::ILog2( int i ) {
+	return ILog2( (float)i );
+}
+
+ID_INLINE int idMath::BitsForFloat( float f ) {
+	return ILog2( f ) + 1;
+}
+
+ID_INLINE int idMath::BitsForInteger( int i ) {
+	return ILog2( (float)i ) + 1;
+}
+
+ID_INLINE int idMath::MaskForFloatSign( float f ) {
+	return ( (*reinterpret_cast<int *>(&f)) >> 31 );
+}
+
+ID_INLINE int idMath::MaskForIntegerSign( int i ) {
+	return ( i >> 31 );
+}
+
+ID_INLINE int idMath::FloorPowerOfTwo( int x ) {
+	return CeilPowerOfTwo( x ) >> 1;
+}
+
+ID_INLINE int idMath::CeilPowerOfTwo( int x ) {
+	x--;
+	x |= x >> 1;
+	x |= x >> 2;
+	x |= x >> 4;
+	x |= x >> 8;
+	x |= x >> 16;
+	x++;
+	return x;
+}
+
+ID_INLINE bool idMath::IsPowerOfTwo( int x ) {
+	return ( x & ( x - 1 ) ) == 0 && x > 0;
+}
+
+ID_INLINE int idMath::BitCount( int x ) {
+	x -= ( ( x >> 1 ) & 0x55555555 );
+	x = ( ( ( x >> 2 ) & 0x33333333 ) + ( x & 0x33333333 ) );
+	x = ( ( ( x >> 4 ) + x ) & 0x0f0f0f0f );
+	x += ( x >> 8 );
+	return ( ( x + ( x >> 16 ) ) & 0x0000003f );
+}
+
+ID_INLINE int idMath::BitReverse( int x ) {
+	x = ( ( ( x >> 1 ) & 0x55555555 ) | ( ( x & 0x55555555 ) << 1 ) );
+	x = ( ( ( x >> 2 ) & 0x33333333 ) | ( ( x & 0x33333333 ) << 2 ) );
+	x = ( ( ( x >> 4 ) & 0x0f0f0f0f ) | ( ( x & 0x0f0f0f0f ) << 4 ) );
+	x = ( ( ( x >> 8 ) & 0x00ff00ff ) | ( ( x & 0x00ff00ff ) << 8 ) );
+	return ( ( x >> 16 ) | ( x << 16 ) );
+}
+
+ID_INLINE int idMath::Abs( int x ) {
+   int y = x >> 31;
+   return ( ( x ^ y ) - y );
+}
+
+ID_INLINE float idMath::Fabs( float f ) {
+	int tmp = *reinterpret_cast<int *>( &f );
+	tmp &= 0x7FFFFFFF;
+	return *reinterpret_cast<float *>( &tmp );
+}
+
+ID_INLINE float idMath::Floor( float f ) {
+	return floorf( f );
+}
+
+ID_INLINE float idMath::Ceil( float f ) {
+	return ceilf( f );
+}
+
+ID_INLINE float idMath::Rint( float f ) {
+	return floorf( f + 0.5f );
+}
+
+ID_INLINE int idMath::Ftoi( float f ) {
+	return (int) f;
+}
+
+ID_INLINE int idMath::FtoiFast( float f ) {
+#ifdef _WIN32
+	int i;
+	__asm fld		f
+	__asm fistp		i		// use default rouding mode (round nearest)
+	return i;
+#elif 0						// round chop (C/C++ standard)
+	int i, s, e, m, shift;
+	i = *reinterpret_cast<int *>(&f);
+	s = i >> IEEE_FLT_SIGN_BIT;
+	e = ( ( i >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+	m = ( i & ( ( 1 << IEEE_FLT_MANTISSA_BITS ) - 1 ) ) | ( 1 << IEEE_FLT_MANTISSA_BITS );
+	shift = e - IEEE_FLT_MANTISSA_BITS;
+	return ( ( ( ( m >> -shift ) | ( m << shift ) ) & ~( e >> 31 ) ) ^ s ) - s;
+//#elif defined( __i386__ )
+#elif 0
+	int i = 0;
+	__asm__ __volatile__ (
+						  "fld %1\n" \
+						  "fistp %0\n" \
+						  : "=m" (i) \
+						  : "m" (f) );
+	return i;
+#else
+	return (int) f;
+#endif
+}
+
+ID_INLINE unsigned long idMath::Ftol( float f ) {
+	return (unsigned long) f;
+}
+
+ID_INLINE unsigned long idMath::FtolFast( float f ) {
+#ifdef _WIN32
+	// FIXME: this overflows on 31bits still .. same as FtoiFast
+	unsigned long i;
+	__asm fld		f
+	__asm fistp		i		// use default rouding mode (round nearest)
+	return i;
+#elif 0						// round chop (C/C++ standard)
+	int i, s, e, m, shift;
+	i = *reinterpret_cast<int *>(&f);
+	s = i >> IEEE_FLT_SIGN_BIT;
+	e = ( ( i >> IEEE_FLT_MANTISSA_BITS ) & ( ( 1 << IEEE_FLT_EXPONENT_BITS ) - 1 ) ) - IEEE_FLT_EXPONENT_BIAS;
+	m = ( i & ( ( 1 << IEEE_FLT_MANTISSA_BITS ) - 1 ) ) | ( 1 << IEEE_FLT_MANTISSA_BITS );
+	shift = e - IEEE_FLT_MANTISSA_BITS;
+	return ( ( ( ( m >> -shift ) | ( m << shift ) ) & ~( e >> 31 ) ) ^ s ) - s;
+//#elif defined( __i386__ )
+#elif 0
+	// for some reason, on gcc I need to make sure i == 0 before performing a fistp
+	int i = 0;
+	__asm__ __volatile__ (
+						  "fld %1\n" \
+						  "fistp %0\n" \
+						  : "=m" (i) \
+						  : "m" (f) );
+	return i;
+#else
+	return (unsigned long) f;
+#endif
+}
+
+ID_INLINE signed char idMath::ClampChar( int i ) {
+	if ( i < -128 ) {
+		return -128;
+	}
+	if ( i > 127 ) {
+		return 127;
+	}
+	return i;
+}
+
+ID_INLINE signed short idMath::ClampShort( int i ) {
+	if ( i < -32768 ) {
+		return -32768;
+	}
+	if ( i > 32767 ) {
+		return 32767;
+	}
+	return i;
+}
+
+ID_INLINE int idMath::ClampInt( int min, int max, int value ) {
+	if ( value < min ) {
+		return min;
+	}
+	if ( value > max ) {
+		return max;
+	}
+	return value;
+}
+
+ID_INLINE float idMath::ClampFloat( float min, float max, float value ) {
+	if ( value < min ) {
+		return min;
+	}
+	if ( value > max ) {
+		return max;
+	}
+	return value;
+}
+
+ID_INLINE float idMath::AngleNormalize360( float angle ) {
+	if ( ( angle >= 360.0f ) || ( angle < 0.0f ) ) {
+		angle -= floor( angle / 360.0f ) * 360.0f;
+	}
+	return angle;
+}
+
+ID_INLINE float idMath::AngleNormalize180( float angle ) {
+	angle = AngleNormalize360( angle );
+	if ( angle > 180.0f ) {
+		angle -= 360.0f;
+	}
+	return angle;
+}
+
+ID_INLINE float idMath::AngleDelta( float angle1, float angle2 ) {
+	return AngleNormalize180( angle1 - angle2 );
+}
+
+ID_INLINE int idMath::FloatHash( const float *array, const int numFloats ) {
+	int i, hash = 0;
+	const int *ptr;
+
+	ptr = reinterpret_cast<const int *>( array );
+	for ( i = 0; i < numFloats; i++ ) {
+		hash ^= ptr[i];
+	}
+	return hash;
+}
+
+#endif /* !__MATH_MATH_H__ */

neo/idlib/math/Ode.cpp

@@ -0,0 +1,355 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+//===============================================================
+//
+//	idODE_Euler
+//
+//===============================================================
+
+/*
+=============
+idODE_Euler::idODE_Euler
+=============
+*/
+idODE_Euler::idODE_Euler( const int dim, deriveFunction_t dr, const void *ud ) {
+	dimension = dim;
+	derivatives = new float[dim];
+	derive = dr;
+	userData = ud;
+}
+
+/*
+=============
+idODE_Euler::~idODE_Euler
+=============
+*/
+idODE_Euler::~idODE_Euler( void ) {
+	delete[] derivatives;
+}
+
+/*
+=============
+idODE_Euler::Evaluate
+=============
+*/
+float idODE_Euler::Evaluate( const float *state, float *newState, float t0, float t1 ) {
+	float delta;
+	int i;
+
+	derive( t0, userData, state, derivatives );
+	delta = t1 - t0;
+	for ( i = 0; i < dimension; i++ ) {
+		newState[i] = state[i] + delta * derivatives[i];
+	}
+	return delta;
+}
+
+//===============================================================
+//
+//	idODE_Midpoint
+//
+//===============================================================
+
+/*
+=============
+idODE_Midpoint::idODE_Midpoint
+=============
+*/
+idODE_Midpoint::idODE_Midpoint( const int dim, deriveFunction_t dr, const void *ud ) {
+	dimension = dim;
+	tmpState = new float[dim];
+	derivatives = new float[dim];
+	derive = dr;
+	userData = ud;
+}
+
+/*
+=============
+idODE_Midpoint::~idODE_Midpoint
+=============
+*/
+idODE_Midpoint::~idODE_Midpoint( void ) {
+	delete tmpState;
+	delete derivatives;
+}
+
+/*
+=============
+idODE_Midpoint::~Evaluate
+=============
+*/
+float idODE_Midpoint::Evaluate( const float *state, float *newState, float t0, float t1 ) {
+	double delta, halfDelta;
+    int i;
+
+	delta = t1 - t0;
+	halfDelta = delta * 0.5;
+    // first step
+	derive( t0, userData, state, derivatives );
+	for ( i = 0; i < dimension; i++ ) {
+		tmpState[i] = state[i] + halfDelta * derivatives[i];
+	}
+    // second step
+	derive( t0 + halfDelta, userData, tmpState, derivatives );
+
+	for ( i = 0; i < dimension; i++ ) {
+		newState[i] = state[i] + delta * derivatives[i];
+	}
+	return delta;
+}
+
+//===============================================================
+//
+//	idODE_RK4
+//
+//===============================================================
+
+/*
+=============
+idODE_RK4::idODE_RK4
+=============
+*/
+idODE_RK4::idODE_RK4( const int dim, deriveFunction_t dr, const void *ud ) {
+	dimension = dim;
+	derive = dr;
+	userData = ud;
+	tmpState = new float[dim];
+	d1 = new float[dim];
+	d2 = new float[dim];
+	d3 = new float[dim];
+	d4 = new float[dim];
+}
+
+/*
+=============
+idODE_RK4::~idODE_RK4
+=============
+*/
+idODE_RK4::~idODE_RK4( void ) {
+	delete tmpState;
+	delete d1;
+	delete d2;
+	delete d3;
+	delete d4;
+}
+
+/*
+=============
+idODE_RK4::Evaluate
+=============
+*/
+float idODE_RK4::Evaluate( const float *state, float *newState, float t0, float t1 ) {
+	double delta, halfDelta, sixthDelta;
+	int i;
+
+	delta = t1 - t0;
+	halfDelta = delta * 0.5;
+	// first step
+	derive( t0, userData, state, d1 );
+	for ( i = 0; i < dimension; i++ ) {
+		tmpState[i] = state[i] + halfDelta * d1[i];
+	}
+	// second step
+	derive( t0 + halfDelta, userData, tmpState, d2 );
+	for ( i = 0; i < dimension; i++ ) {
+		tmpState[i] = state[i] + halfDelta * d2[i];
+	}
+	// third step
+	derive( t0 + halfDelta, userData, tmpState, d3 );
+	for ( i = 0; i < dimension; i++ ) {
+		tmpState[i] = state[i] + delta * d3[i];
+	}
+	// fourth step
+	derive( t0 + delta, userData, tmpState, d4 );
+
+	sixthDelta = delta * (1.0/6.0);
+	for ( i = 0; i < dimension; i++ ) {
+		newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
+	}
+	return delta;
+}
+
+//===============================================================
+//
+//	idODE_RK4Adaptive
+//
+//===============================================================
+
+/*
+=============
+idODE_RK4Adaptive::idODE_RK4Adaptive
+=============
+*/
+idODE_RK4Adaptive::idODE_RK4Adaptive( const int dim, deriveFunction_t dr, const void *ud ) {
+	dimension = dim;
+	derive = dr;
+	userData = ud;
+	maxError = 0.01f;
+	tmpState = new float[dim];
+	d1 = new float[dim];
+	d1half = new float [dim];
+	d2 = new float[dim];
+	d3 = new float[dim];
+	d4 = new float[dim];
+}
+
+/*
+=============
+idODE_RK4Adaptive::~idODE_RK4Adaptive
+=============
+*/
+idODE_RK4Adaptive::~idODE_RK4Adaptive( void ) {
+	delete tmpState;
+	delete d1;
+	delete d1half;
+	delete d2;
+	delete d3;
+	delete d4;
+}
+
+/*
+=============
+idODE_RK4Adaptive::SetMaxError
+=============
+*/
+void idODE_RK4Adaptive::SetMaxError( const float err ) {
+	if ( err > 0.0f ) {
+		maxError = err;
+	}
+}
+
+/*
+=============
+idODE_RK4Adaptive::Evaluate
+=============
+*/
+float idODE_RK4Adaptive::Evaluate( const float *state, float *newState, float t0, float t1 ) {
+	double delta, halfDelta, fourthDelta, sixthDelta;
+	double error, max;
+	int i, n;
+
+	delta = t1 - t0;
+
+	for ( n = 0; n < 4; n++ ) {
+
+		halfDelta = delta * 0.5;
+		fourthDelta = delta * 0.25;
+
+		// first step of first half delta
+		derive( t0, userData, state, d1 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + fourthDelta * d1[i];
+		}
+		// second step of first half delta
+		derive( t0 + fourthDelta, userData, tmpState, d2 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + fourthDelta * d2[i];
+		}
+		// third step of first half delta
+		derive( t0 + fourthDelta, userData, tmpState, d3 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + halfDelta * d3[i];
+		}
+		// fourth step of first half delta
+		derive( t0 + halfDelta, userData, tmpState, d4 );
+
+		sixthDelta = halfDelta * (1.0/6.0);
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
+		}
+
+		// first step of second half delta
+		derive( t0 + halfDelta, userData, tmpState, d1half );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + fourthDelta * d1half[i];
+		}
+		// second step of second half delta
+		derive( t0 + halfDelta + fourthDelta, userData, tmpState, d2 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + fourthDelta * d2[i];
+		}
+		// third step of second half delta
+		derive( t0 + halfDelta + fourthDelta, userData, tmpState, d3 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + halfDelta * d3[i];
+		}
+		// fourth step of second half delta
+		derive( t0 + delta, userData, tmpState, d4 );
+
+		sixthDelta = halfDelta * (1.0/6.0);
+		for ( i = 0; i < dimension; i++ ) {
+			newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
+		}
+
+		// first step of full delta
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + halfDelta * d1[i];
+		}
+		// second step of full delta
+		derive( t0 + halfDelta, userData, tmpState, d2 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + halfDelta * d2[i];
+		}
+		// third step of full delta
+		derive( t0 + halfDelta, userData, tmpState, d3 );
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + delta * d3[i];
+		}
+		// fourth step of full delta
+		derive( t0 + delta, userData, tmpState, d4 );
+
+		sixthDelta = delta * (1.0/6.0);
+		for ( i = 0; i < dimension; i++ ) {
+			tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
+		}
+
+		// get max estimated error
+        max = 0.0;
+		for ( i = 0; i < dimension; i++ ) {
+			error = idMath::Fabs( (newState[i] - tmpState[i]) / (delta * d1[i] + 1e-10) );
+			if ( error > max ) {
+				max = error;
+			}
+        }
+		error = max / maxError;
+
+        if ( error <= 1.0f ) {
+			return delta * 4.0;
+		}
+		if ( delta <= 1e-7 ) {
+			return delta;
+		}
+		delta *= 0.25;
+	}
+	return delta;
+}
+

neo/idlib/math/Ode.h

@@ -0,0 +1,146 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_ODE_H__
+#define __MATH_ODE_H__
+
+/*
+===============================================================================
+
+	Numerical solvers for ordinary differential equations.
+
+===============================================================================
+*/
+
+
+//===============================================================
+//
+//	idODE
+//
+//===============================================================
+
+typedef void (*deriveFunction_t)( const float t, const void *userData, const float *state, float *derivatives );
+
+class idODE {
+
+public:
+	virtual				~idODE( void ) {}
+
+	virtual float		Evaluate( const float *state, float *newState, float t0, float t1 ) = 0;
+
+protected:
+	int					dimension;		// dimension in floats allocated for
+	deriveFunction_t	derive;			// derive function
+	const void *		userData;		// client data
+};
+
+//===============================================================
+//
+//	idODE_Euler
+//
+//===============================================================
+
+class idODE_Euler : public idODE {
+
+public:
+						idODE_Euler( const int dim, const deriveFunction_t dr, const void *ud );
+	virtual				~idODE_Euler( void );
+
+	virtual float		Evaluate( const float *state, float *newState, float t0, float t1 );
+
+protected:
+	float *				derivatives;	// space to store derivatives
+};
+
+//===============================================================
+//
+//	idODE_Midpoint
+//
+//===============================================================
+
+class idODE_Midpoint : public idODE {
+
+public:
+						idODE_Midpoint( const int dim, const deriveFunction_t dr, const void *ud );
+	virtual				~idODE_Midpoint( void );
+
+	virtual float		Evaluate( const float *state, float *newState, float t0, float t1 );
+
+protected:
+	float *				tmpState;
+	float *				derivatives;	// space to store derivatives
+};
+
+//===============================================================
+//
+//	idODE_RK4
+//
+//===============================================================
+
+class idODE_RK4 : public idODE {
+
+public:
+						idODE_RK4( const int dim, const deriveFunction_t dr, const void *ud );
+	virtual				~idODE_RK4( void );
+
+	virtual float		Evaluate( const float *state, float *newState, float t0, float t1 );
+
+protected:
+	float *				tmpState;
+	float *				d1;				// derivatives
+	float *				d2;
+	float *				d3;
+	float *				d4;
+};
+
+//===============================================================
+//
+//	idODE_RK4Adaptive
+//
+//===============================================================
+
+class idODE_RK4Adaptive : public idODE {
+
+public:
+						idODE_RK4Adaptive( const int dim, const deriveFunction_t dr, const void *ud );
+	virtual				~idODE_RK4Adaptive( void );
+
+	virtual float		Evaluate( const float *state, float *newState, float t0, float t1 );
+	void				SetMaxError( const float err );
+
+protected:
+	float				maxError;		// maximum allowed error
+	float *				tmpState;
+	float *				d1;				// derivatives
+	float *				d1half;
+	float *				d2;
+	float *				d3;
+	float *				d4;
+};
+
+#endif /* !__MATH_ODE_H__ */

neo/idlib/math/Plane.cpp

@@ -0,0 +1,154 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+idPlane plane_origin( 0.0f, 0.0f, 0.0f, 0.0f );
+
+/*
+================
+idPlane::Type
+================
+*/
+int idPlane::Type( void ) const {
+	if ( Normal()[0] == 0.0f ) {
+		if ( Normal()[1] == 0.0f ) {
+			return Normal()[2] > 0.0f ? PLANETYPE_Z : PLANETYPE_NEGZ;
+		}
+		else if ( Normal()[2] == 0.0f ) {
+			return Normal()[1] > 0.0f ? PLANETYPE_Y : PLANETYPE_NEGY;
+		}
+		else {
+			return PLANETYPE_ZEROX;
+		}
+	}
+	else if ( Normal()[1] == 0.0f ) {
+		if ( Normal()[2] == 0.0f ) {
+			return Normal()[0] > 0.0f ? PLANETYPE_X : PLANETYPE_NEGX;
+		}
+		else {
+			return PLANETYPE_ZEROY;
+		}
+	}
+	else if ( Normal()[2] == 0.0f ) {
+		return PLANETYPE_ZEROZ;
+	}
+	else {
+		return PLANETYPE_NONAXIAL;
+	}
+}
+
+/*
+================
+idPlane::HeightFit
+================
+*/
+bool idPlane::HeightFit( const idVec3 *points, const int numPoints ) {
+	int i;
+	float sumXX = 0.0f, sumXY = 0.0f, sumXZ = 0.0f;
+	float sumYY = 0.0f, sumYZ = 0.0f;
+	idVec3 sum, average, dir;
+
+	if ( numPoints == 1 ) {
+		a = 0.0f;
+		b = 0.0f;
+		c = 1.0f;
+		d = -points[0].z;
+		return true;
+	}
+	if ( numPoints == 2 ) {
+		dir = points[1] - points[0];
+		Normal() = dir.Cross( idVec3( 0, 0, 1 ) ).Cross( dir );
+		Normalize();
+		d = -( Normal() * points[0] );
+		return true;
+	}
+
+	sum.Zero();
+	for ( i = 0; i < numPoints; i++) {
+		sum += points[i];
+	}
+	average = sum / numPoints;
+
+	for ( i = 0; i < numPoints; i++ ) {
+		dir = points[i] - average;
+		sumXX += dir.x * dir.x;
+		sumXY += dir.x * dir.y;
+		sumXZ += dir.x * dir.z;
+		sumYY += dir.y * dir.y;
+		sumYZ += dir.y * dir.z;
+	}
+
+	idMat2 m( sumXX, sumXY, sumXY, sumYY );
+	if ( !m.InverseSelf() ) {
+		return false;
+	}
+
+	a = - sumXZ * m[0][0] - sumYZ * m[0][1];
+	b = - sumXZ * m[1][0] - sumYZ * m[1][1];
+	c = 1.0f;
+	Normalize();
+	d = -( a * average.x + b * average.y + c * average.z );
+	return true;
+}
+
+/*
+================
+idPlane::PlaneIntersection
+================
+*/
+bool idPlane::PlaneIntersection( const idPlane &plane, idVec3 &start, idVec3 &dir ) const {
+	double n00, n01, n11, det, invDet, f0, f1;
+
+	n00 = Normal().LengthSqr();
+	n01 = Normal() * plane.Normal();
+	n11 = plane.Normal().LengthSqr();
+	det = n00 * n11 - n01 * n01;
+
+	if ( idMath::Fabs(det) < 1e-6f ) {
+		return false;
+	}
+
+	invDet = 1.0f / det;
+	f0 = ( n01 * plane.d - n11 * d ) * invDet;
+	f1 = ( n01 * d - n00 * plane.d ) * invDet;
+
+	dir = Normal().Cross( plane.Normal() );
+	start = f0 * Normal() + f1 * plane.Normal();
+	return true;
+}
+
+/*
+=============
+idPlane::ToString
+=============
+*/
+const char *idPlane::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}

neo/idlib/math/Plane.h

@@ -0,0 +1,391 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_PLANE_H__
+#define __MATH_PLANE_H__
+
+/*
+===============================================================================
+
+	3D plane with equation: a * x + b * y + c * z + d = 0
+
+===============================================================================
+*/
+
+
+class idVec3;
+class idMat3;
+
+#define	ON_EPSILON					0.1f
+#define DEGENERATE_DIST_EPSILON		1e-4f
+
+#define	SIDE_FRONT					0
+#define	SIDE_BACK					1
+#define	SIDE_ON						2
+#define	SIDE_CROSS					3
+
+// plane sides
+#define PLANESIDE_FRONT				0
+#define PLANESIDE_BACK				1
+#define PLANESIDE_ON				2
+#define PLANESIDE_CROSS				3
+
+// plane types
+#define PLANETYPE_X					0
+#define PLANETYPE_Y					1
+#define PLANETYPE_Z					2
+#define PLANETYPE_NEGX				3
+#define PLANETYPE_NEGY				4
+#define PLANETYPE_NEGZ				5
+#define PLANETYPE_TRUEAXIAL			6	// all types < 6 are true axial planes
+#define PLANETYPE_ZEROX				6
+#define PLANETYPE_ZEROY				7
+#define PLANETYPE_ZEROZ				8
+#define PLANETYPE_NONAXIAL			9
+
+class idPlane {
+public:
+					idPlane( void );
+					idPlane( float a, float b, float c, float d );
+					idPlane( const idVec3 &normal, const float dist );
+
+	float			operator[]( int index ) const;
+	float &			operator[]( int index );
+	idPlane			operator-() const;						// flips plane
+	idPlane &		operator=( const idVec3 &v );			// sets normal and sets idPlane::d to zero
+	idPlane			operator+( const idPlane &p ) const;	// add plane equations
+	idPlane			operator-( const idPlane &p ) const;	// subtract plane equations
+	idPlane &		operator*=( const idMat3 &m );			// Normal() *= m
+
+	bool			Compare( const idPlane &p ) const;						// exact compare, no epsilon
+	bool			Compare( const idPlane &p, const float epsilon ) const;	// compare with epsilon
+	bool			Compare( const idPlane &p, const float normalEps, const float distEps ) const;	// compare with epsilon
+	bool			operator==(	const idPlane &p ) const;					// exact compare, no epsilon
+	bool			operator!=(	const idPlane &p ) const;					// exact compare, no epsilon
+
+	void			Zero( void );							// zero plane
+	void			SetNormal( const idVec3 &normal );		// sets the normal
+	const idVec3 &	Normal( void ) const;					// reference to const normal
+	idVec3 &		Normal( void );							// reference to normal
+	float			Normalize( bool fixDegenerate = true );	// only normalizes the plane normal, does not adjust d
+	bool			FixDegenerateNormal( void );			// fix degenerate normal
+	bool			FixDegeneracies( float distEpsilon );	// fix degenerate normal and dist
+	float			Dist( void ) const;						// returns: -d
+	void			SetDist( const float dist );			// sets: d = -dist
+	int				Type( void ) const;						// returns plane type
+
+	bool			FromPoints( const idVec3 &p1, const idVec3 &p2, const idVec3 &p3, bool fixDegenerate = true );
+	bool			FromVecs( const idVec3 &dir1, const idVec3 &dir2, const idVec3 &p, bool fixDegenerate = true );
+	void			FitThroughPoint( const idVec3 &p );	// assumes normal is valid
+	bool			HeightFit( const idVec3 *points, const int numPoints );
+	idPlane			Translate( const idVec3 &translation ) const;
+	idPlane &		TranslateSelf( const idVec3 &translation );
+	idPlane			Rotate( const idVec3 &origin, const idMat3 &axis ) const;
+	idPlane &		RotateSelf( const idVec3 &origin, const idMat3 &axis );
+
+	float			Distance( const idVec3 &v ) const;
+	int				Side( const idVec3 &v, const float epsilon = 0.0f ) const;
+
+	bool			LineIntersection( const idVec3 &start, const idVec3 &end ) const;
+					// intersection point is start + dir * scale
+	bool			RayIntersection( const idVec3 &start, const idVec3 &dir, float &scale ) const;
+	bool			PlaneIntersection( const idPlane &plane, idVec3 &start, idVec3 &dir ) const;
+
+	int				GetDimension( void ) const;
+
+	const idVec4 &	ToVec4( void ) const;
+	idVec4 &		ToVec4( void );
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+
+private:
+	float			a;
+	float			b;
+	float			c;
+	float			d;
+};
+
+extern idPlane plane_origin;
+#define plane_zero plane_origin
+
+ID_INLINE idPlane::idPlane( void ) {
+}
+
+ID_INLINE idPlane::idPlane( float a, float b, float c, float d ) {
+	this->a = a;
+	this->b = b;
+	this->c = c;
+	this->d = d;
+}
+
+ID_INLINE idPlane::idPlane( const idVec3 &normal, const float dist ) {
+	this->a = normal.x;
+	this->b = normal.y;
+	this->c = normal.z;
+	this->d = -dist;
+}
+
+ID_INLINE float idPlane::operator[]( int index ) const {
+	return ( &a )[ index ];
+}
+
+ID_INLINE float& idPlane::operator[]( int index ) {
+	return ( &a )[ index ];
+}
+
+ID_INLINE idPlane idPlane::operator-() const {
+	return idPlane( -a, -b, -c, -d );
+}
+
+ID_INLINE idPlane &idPlane::operator=( const idVec3 &v ) { 
+	a = v.x;
+	b = v.y;
+	c = v.z;
+	d = 0;
+	return *this;
+}
+
+ID_INLINE idPlane idPlane::operator+( const idPlane &p ) const {
+	return idPlane( a + p.a, b + p.b, c + p.c, d + p.d );
+}
+
+ID_INLINE idPlane idPlane::operator-( const idPlane &p ) const {
+	return idPlane( a - p.a, b - p.b, c - p.c, d - p.d );
+}
+
+ID_INLINE idPlane &idPlane::operator*=( const idMat3 &m ) {
+	Normal() *= m;
+	return *this;
+}
+
+ID_INLINE bool idPlane::Compare( const idPlane &p ) const {
+	return ( a == p.a && b == p.b && c == p.c && d == p.d );
+}
+
+ID_INLINE bool idPlane::Compare( const idPlane &p, const float epsilon ) const {
+	if ( idMath::Fabs( a - p.a ) > epsilon ) {
+		return false;
+	}
+			
+	if ( idMath::Fabs( b - p.b ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( c - p.c ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( d - p.d ) > epsilon ) {
+		return false;
+	}
+
+	return true;
+}
+
+ID_INLINE bool idPlane::Compare( const idPlane &p, const float normalEps, const float distEps ) const {
+	if ( idMath::Fabs( d - p.d ) > distEps ) {
+		return false;
+	}
+	if ( !Normal().Compare( p.Normal(), normalEps ) ) {
+		return false;
+	}
+	return true;
+}
+
+ID_INLINE bool idPlane::operator==( const idPlane &p ) const {
+	return Compare( p );
+}
+
+ID_INLINE bool idPlane::operator!=( const idPlane &p ) const {
+	return !Compare( p );
+}
+
+ID_INLINE void idPlane::Zero( void ) {
+	a = b = c = d = 0.0f;
+}
+
+ID_INLINE void idPlane::SetNormal( const idVec3 &normal ) {
+	a = normal.x;
+	b = normal.y;
+	c = normal.z;
+}
+
+ID_INLINE const idVec3 &idPlane::Normal( void ) const {
+	return *reinterpret_cast<const idVec3 *>(&a);
+}
+
+ID_INLINE idVec3 &idPlane::Normal( void ) {
+	return *reinterpret_cast<idVec3 *>(&a);
+}
+
+ID_INLINE float idPlane::Normalize( bool fixDegenerate ) {
+	float length = reinterpret_cast<idVec3 *>(&a)->Normalize();
+
+	if ( fixDegenerate ) {
+		FixDegenerateNormal();
+	}
+	return length;
+}
+
+ID_INLINE bool idPlane::FixDegenerateNormal( void ) {
+	return Normal().FixDegenerateNormal();
+}
+
+ID_INLINE bool idPlane::FixDegeneracies( float distEpsilon ) {
+	bool fixedNormal = FixDegenerateNormal();
+	// only fix dist if the normal was degenerate
+	if ( fixedNormal ) {
+		if ( idMath::Fabs( d - idMath::Rint( d ) ) < distEpsilon ) {
+			d = idMath::Rint( d );
+		}
+	}
+	return fixedNormal;
+}
+
+ID_INLINE float idPlane::Dist( void ) const {
+	return -d;
+}
+
+ID_INLINE void idPlane::SetDist( const float dist ) {
+	d = -dist;
+}
+
+ID_INLINE bool idPlane::FromPoints( const idVec3 &p1, const idVec3 &p2, const idVec3 &p3, bool fixDegenerate ) {
+	Normal() = (p1 - p2).Cross( p3 - p2 );
+	if ( Normalize( fixDegenerate ) == 0.0f ) {
+		return false;
+	}
+	d = -( Normal() * p2 );
+	return true;
+}
+
+ID_INLINE bool idPlane::FromVecs( const idVec3 &dir1, const idVec3 &dir2, const idVec3 &p, bool fixDegenerate ) {
+	Normal() = dir1.Cross( dir2 );
+	if ( Normalize( fixDegenerate ) == 0.0f ) {
+		return false;
+	}
+	d = -( Normal() * p );
+	return true;
+}
+
+ID_INLINE void idPlane::FitThroughPoint( const idVec3 &p ) {
+	d = -( Normal() * p );
+}
+
+ID_INLINE idPlane idPlane::Translate( const idVec3 &translation ) const {
+	return idPlane( a, b, c, d - translation * Normal() );
+}
+
+ID_INLINE idPlane &idPlane::TranslateSelf( const idVec3 &translation ) {
+	d -= translation * Normal();
+	return *this;
+}
+
+ID_INLINE idPlane idPlane::Rotate( const idVec3 &origin, const idMat3 &axis ) const {
+	idPlane p;
+	p.Normal() = Normal() * axis;
+	p.d = d + origin * Normal() - origin * p.Normal();
+	return p;
+}
+
+ID_INLINE idPlane &idPlane::RotateSelf( const idVec3 &origin, const idMat3 &axis ) {
+	d += origin * Normal();
+	Normal() *= axis;
+	d -= origin * Normal();
+	return *this;
+}
+
+ID_INLINE float idPlane::Distance( const idVec3 &v ) const {
+	return a * v.x + b * v.y + c * v.z + d;
+}
+
+ID_INLINE int idPlane::Side( const idVec3 &v, const float epsilon ) const {
+	float dist = Distance( v );
+	if ( dist > epsilon ) {
+		return PLANESIDE_FRONT;
+	}
+	else if ( dist < -epsilon ) {
+		return PLANESIDE_BACK;
+	}
+	else {
+		return PLANESIDE_ON;
+	}
+}
+
+ID_INLINE bool idPlane::LineIntersection( const idVec3 &start, const idVec3 &end ) const {
+	float d1, d2, fraction;
+
+	d1 = Normal() * start + d;
+	d2 = Normal() * end + d;
+	if ( d1 == d2 ) {
+		return false;
+	}
+	if ( d1 > 0.0f && d2 > 0.0f ) {
+		return false;
+	}
+	if ( d1 < 0.0f && d2 < 0.0f ) {
+		return false;
+	}
+	fraction = ( d1 / ( d1 - d2 ) );
+	return ( fraction >= 0.0f && fraction <= 1.0f );
+}
+
+ID_INLINE bool idPlane::RayIntersection( const idVec3 &start, const idVec3 &dir, float &scale ) const {
+	float d1, d2;
+
+	d1 = Normal() * start + d;
+	d2 = Normal() * dir;
+	if ( d2 == 0.0f ) {
+		return false;
+	}
+	scale = -( d1 / d2 );
+	return true;
+}
+
+ID_INLINE int idPlane::GetDimension( void ) const {
+	return 4;
+}
+
+ID_INLINE const idVec4 &idPlane::ToVec4( void ) const {
+	return *reinterpret_cast<const idVec4 *>(&a);
+}
+
+ID_INLINE idVec4 &idPlane::ToVec4( void ) {
+	return *reinterpret_cast<idVec4 *>(&a);
+}
+
+ID_INLINE const float *idPlane::ToFloatPtr( void ) const {
+	return reinterpret_cast<const float *>(&a);
+}
+
+ID_INLINE float *idPlane::ToFloatPtr( void ) {
+	return reinterpret_cast<float *>(&a);
+}
+
+#endif /* !__MATH_PLANE_H__ */

neo/idlib/math/Pluecker.cpp

@@ -0,0 +1,86 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+idPluecker pluecker_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );
+
+/*
+================
+idPluecker::FromPlanes
+
+  pluecker coordinate for the intersection of two planes
+================
+*/
+bool idPluecker::FromPlanes( const idPlane &p1, const idPlane &p2 ) {
+
+	p[0] = -( p1[2] * -p2[3] - p2[2] * -p1[3] );
+	p[1] = -( p2[1] * -p1[3] - p1[1] * -p2[3] );
+	p[2] = p1[1] * p2[2] - p2[1] * p1[2];
+
+	p[3] = -( p1[0] * -p2[3] - p2[0] * -p1[3] );
+	p[4] = p1[0] * p2[1] - p2[0] * p1[1];
+	p[5] = p1[0] * p2[2] - p2[0] * p1[2];
+
+	return ( p[2] != 0.0f || p[5] != 0.0f || p[4] != 0.0f );
+}
+
+/*
+================
+idPluecker::Distance3DSqr
+
+  calculates square of shortest distance between the two
+  3D lines represented by their pluecker coordinates
+================
+*/
+float idPluecker::Distance3DSqr( const idPluecker &a ) const {
+	float d, s;
+	idVec3 dir;
+
+	dir[0] = -a.p[5] *  p[4] -  a.p[4] * -p[5];
+	dir[1] =  a.p[4] *  p[2] -  a.p[2] *  p[4];
+	dir[2] =  a.p[2] * -p[5] - -a.p[5] *  p[2];
+	if ( dir[0] == 0.0f && dir[1] == 0.0f && dir[2] == 0.0f ) {
+		return -1.0f;	// FIXME: implement for parallel lines
+	}
+	d = a.p[4] * ( p[2]*dir[1] - -p[5]*dir[0]) +
+		a.p[5] * ( p[2]*dir[2] -  p[4]*dir[0]) +
+		a.p[2] * (-p[5]*dir[2] -  p[4]*dir[1]);
+	s = PermutedInnerProduct( a ) / d;
+	return ( dir * dir ) * ( s * s );
+}
+
+/*
+=============
+idPluecker::ToString
+=============
+*/
+const char *idPluecker::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}

neo/idlib/math/Pluecker.h

@@ -0,0 +1,368 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_PLUECKER_H__
+#define __MATH_PLUECKER_H__
+
+/*
+===============================================================================
+
+	Pluecker coordinate
+
+===============================================================================
+*/
+
+class idPluecker {
+public:	
+					idPluecker( void );
+					explicit idPluecker( const float *a );
+					explicit idPluecker( const idVec3 &start, const idVec3 &end );
+					explicit idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
+
+	float			operator[]( const int index ) const;
+	float &			operator[]( const int index );
+	idPluecker		operator-() const;											// flips the direction
+	idPluecker		operator*( const float a ) const;
+	idPluecker		operator/( const float a ) const;
+	float			operator*( const idPluecker &a ) const;						// permuted inner product
+	idPluecker		operator-( const idPluecker &a ) const;
+	idPluecker		operator+( const idPluecker &a ) const;
+	idPluecker &	operator*=( const float a );
+	idPluecker &	operator/=( const float a );
+	idPluecker &	operator+=( const idPluecker &a );
+	idPluecker &	operator-=( const idPluecker &a );
+
+	bool			Compare( const idPluecker &a ) const;						// exact compare, no epsilon
+	bool			Compare( const idPluecker &a, const float epsilon ) const;	// compare with epsilon
+	bool			operator==(	const idPluecker &a ) const;					// exact compare, no epsilon
+	bool			operator!=(	const idPluecker &a ) const;					// exact compare, no epsilon
+
+	void 			Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
+	void			Zero( void );
+
+	void			FromLine( const idVec3 &start, const idVec3 &end );			// pluecker from line
+	void			FromRay( const idVec3 &start, const idVec3 &dir );			// pluecker from ray
+	bool			FromPlanes( const idPlane &p1, const idPlane &p2 );			// pluecker from intersection of planes
+	bool			ToLine( idVec3 &start, idVec3 &end ) const;					// pluecker to line
+	bool			ToRay( idVec3 &start, idVec3 &dir ) const;					// pluecker to ray
+	void			ToDir( idVec3 &dir ) const;									// pluecker to direction
+	float			PermutedInnerProduct( const idPluecker &a ) const;			// pluecker permuted inner product
+	float			Distance3DSqr( const idPluecker &a ) const;					// pluecker line distance
+
+	float			Length( void ) const;										// pluecker length
+	float			LengthSqr( void ) const;									// pluecker squared length
+	idPluecker		Normalize( void ) const;									// pluecker normalize
+	float			NormalizeSelf( void );										// pluecker normalize
+
+	int				GetDimension( void ) const;
+
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+
+private:
+	float			p[6];
+};
+
+extern idPluecker pluecker_origin;
+#define pluecker_zero pluecker_origin
+
+ID_INLINE idPluecker::idPluecker( void ) {
+}
+
+ID_INLINE idPluecker::idPluecker( const float *a ) {
+	memcpy( p, a, 6 * sizeof( float ) );
+}
+
+ID_INLINE idPluecker::idPluecker( const idVec3 &start, const idVec3 &end ) {
+	FromLine( start, end );
+}
+
+ID_INLINE idPluecker::idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
+	p[0] = a1;
+	p[1] = a2;
+	p[2] = a3;
+	p[3] = a4;
+	p[4] = a5;
+	p[5] = a6;
+}
+
+ID_INLINE idPluecker idPluecker::operator-() const {
+	return idPluecker( -p[0], -p[1], -p[2], -p[3], -p[4], -p[5] );
+}
+
+ID_INLINE float idPluecker::operator[]( const int index ) const {
+	return p[index];
+}
+
+ID_INLINE float &idPluecker::operator[]( const int index ) {
+	return p[index];
+}
+
+ID_INLINE idPluecker idPluecker::operator*( const float a ) const {
+	return idPluecker( p[0]*a, p[1]*a, p[2]*a, p[3]*a, p[4]*a, p[5]*a );
+}
+
+ID_INLINE float idPluecker::operator*( const idPluecker &a ) const {
+	return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
+}
+
+ID_INLINE idPluecker idPluecker::operator/( const float a ) const {
+	float inva;
+
+	assert( a != 0.0f );
+	inva = 1.0f / a;
+	return idPluecker( p[0]*inva, p[1]*inva, p[2]*inva, p[3]*inva, p[4]*inva, p[5]*inva );
+}
+
+ID_INLINE idPluecker idPluecker::operator+( const idPluecker &a ) const {
+	return idPluecker( p[0] + a[0], p[1] + a[1], p[2] + a[2], p[3] + a[3], p[4] + a[4], p[5] + a[5] );
+}
+
+ID_INLINE idPluecker idPluecker::operator-( const idPluecker &a ) const {
+	return idPluecker( p[0] - a[0], p[1] - a[1], p[2] - a[2], p[3] - a[3], p[4] - a[4], p[5] - a[5] );
+}
+
+ID_INLINE idPluecker &idPluecker::operator*=( const float a ) {
+	p[0] *= a;
+	p[1] *= a;
+	p[2] *= a;
+	p[3] *= a;
+	p[4] *= a;
+	p[5] *= a;
+	return *this;
+}
+
+ID_INLINE idPluecker &idPluecker::operator/=( const float a ) {
+	float inva;
+
+	assert( a != 0.0f );
+	inva = 1.0f / a;
+	p[0] *= inva;
+	p[1] *= inva;
+	p[2] *= inva;
+	p[3] *= inva;
+	p[4] *= inva;
+	p[5] *= inva;
+	return *this;
+}
+
+ID_INLINE idPluecker &idPluecker::operator+=( const idPluecker &a ) {
+	p[0] += a[0];
+	p[1] += a[1];
+	p[2] += a[2];
+	p[3] += a[3];
+	p[4] += a[4];
+	p[5] += a[5];
+	return *this;
+}
+
+ID_INLINE idPluecker &idPluecker::operator-=( const idPluecker &a ) {
+	p[0] -= a[0];
+	p[1] -= a[1];
+	p[2] -= a[2];
+	p[3] -= a[3];
+	p[4] -= a[4];
+	p[5] -= a[5];
+	return *this;
+}
+
+ID_INLINE bool idPluecker::Compare( const idPluecker &a ) const {
+	return ( ( p[0] == a[0] ) && ( p[1] == a[1] ) && ( p[2] == a[2] ) &&
+			( p[3] == a[3] ) && ( p[4] == a[4] ) && ( p[5] == a[5] ) );
+}
+
+ID_INLINE bool idPluecker::Compare( const idPluecker &a, const float epsilon ) const {
+	if ( idMath::Fabs( p[0] - a[0] ) > epsilon ) {
+		return false;
+	}
+			
+	if ( idMath::Fabs( p[1] - a[1] ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( p[2] - a[2] ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( p[3] - a[3] ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( p[4] - a[4] ) > epsilon ) {
+		return false;
+	}
+
+	if ( idMath::Fabs( p[5] - a[5] ) > epsilon ) {
+		return false;
+	}
+
+	return true;
+}
+
+ID_INLINE bool idPluecker::operator==( const idPluecker &a ) const {
+	return Compare( a );
+}
+
+ID_INLINE bool idPluecker::operator!=( const idPluecker &a ) const {
+	return !Compare( a );
+}
+
+ID_INLINE void idPluecker::Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
+	p[0] = a1;
+	p[1] = a2;
+	p[2] = a3;
+	p[3] = a4;
+	p[4] = a5;
+	p[5] = a6;
+}
+
+ID_INLINE void idPluecker::Zero( void ) {
+	p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0.0f;
+}
+
+ID_INLINE void idPluecker::FromLine( const idVec3 &start, const idVec3 &end ) {
+	p[0] = start[0] * end[1] - end[0] * start[1];
+	p[1] = start[0] * end[2] - end[0] * start[2];
+	p[2] = start[0] - end[0];
+	p[3] = start[1] * end[2] - end[1] * start[2];
+	p[4] = start[2] - end[2];
+	p[5] = end[1] - start[1];
+}
+
+ID_INLINE void idPluecker::FromRay( const idVec3 &start, const idVec3 &dir ) {
+	p[0] = start[0] * dir[1] - dir[0] * start[1];
+	p[1] = start[0] * dir[2] - dir[0] * start[2];
+	p[2] = -dir[0];
+	p[3] = start[1] * dir[2] - dir[1] * start[2];
+	p[4] = -dir[2];
+	p[5] = dir[1];
+}
+
+ID_INLINE bool idPluecker::ToLine( idVec3 &start, idVec3 &end ) const {
+	idVec3 dir1, dir2;
+	float d;
+
+	dir1[0] = p[3];
+	dir1[1] = -p[1];
+	dir1[2] = p[0];
+
+	dir2[0] = -p[2];
+	dir2[1] = p[5];
+	dir2[2] = -p[4];
+
+	d = dir2 * dir2;
+	if ( d == 0.0f ) {
+		return false; // pluecker coordinate does not represent a line
+	}
+
+	start = dir2.Cross(dir1) * (1.0f / d);
+	end = start + dir2;
+	return true;
+}
+
+ID_INLINE bool idPluecker::ToRay( idVec3 &start, idVec3 &dir ) const {
+	idVec3 dir1;
+	float d;
+
+	dir1[0] = p[3];
+	dir1[1] = -p[1];
+	dir1[2] = p[0];
+
+	dir[0] = -p[2];
+	dir[1] = p[5];
+	dir[2] = -p[4];
+
+	d = dir * dir;
+	if ( d == 0.0f ) {
+		return false; // pluecker coordinate does not represent a line
+	}
+
+	start = dir.Cross(dir1) * (1.0f / d);
+	return true;
+}
+
+ID_INLINE void idPluecker::ToDir( idVec3 &dir ) const {
+	dir[0] = -p[2];
+	dir[1] = p[5];
+	dir[2] = -p[4];
+}
+
+ID_INLINE float idPluecker::PermutedInnerProduct( const idPluecker &a ) const {
+	return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
+}
+
+ID_INLINE float idPluecker::Length( void ) const {
+	return ( float )idMath::Sqrt( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
+}
+
+ID_INLINE float idPluecker::LengthSqr( void ) const {
+	return ( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
+}
+
+ID_INLINE float idPluecker::NormalizeSelf( void ) {
+	float l, d;
+
+	l = LengthSqr();
+	if ( l == 0.0f ) {
+		return l; // pluecker coordinate does not represent a line
+	}
+	d = idMath::InvSqrt( l );
+	p[0] *= d;
+	p[1] *= d;
+	p[2] *= d;
+	p[3] *= d;
+	p[4] *= d;
+	p[5] *= d;
+	return d * l;
+}
+
+ID_INLINE idPluecker idPluecker::Normalize( void ) const {
+	float d;
+
+	d = LengthSqr();
+	if ( d == 0.0f ) {
+		return *this; // pluecker coordinate does not represent a line
+	}
+	d = idMath::InvSqrt( d );
+	return idPluecker( p[0]*d, p[1]*d, p[2]*d, p[3]*d, p[4]*d, p[5]*d );
+}
+
+ID_INLINE int idPluecker::GetDimension( void ) const {
+	return 6;
+}
+
+ID_INLINE const float *idPluecker::ToFloatPtr( void ) const {
+	return p;
+}
+
+ID_INLINE float *idPluecker::ToFloatPtr( void ) {
+	return p;
+}
+
+#endif /* !__MATH_PLUECKER_H__ */

neo/idlib/math/Polynomial.cpp

@@ -0,0 +1,242 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+const float EPSILON		= 1e-6f;
+
+/*
+=============
+idPolynomial::Laguer
+=============
+*/
+int idPolynomial::Laguer( const idComplex *coef, const int degree, idComplex &x ) const {
+	const int MT = 10, MAX_ITERATIONS = MT * 8;
+	static const float frac[] = { 0.0f, 0.5f, 0.25f, 0.75f, 0.13f, 0.38f, 0.62f, 0.88f, 1.0f };
+	int i, j;
+	float abx, abp, abm, err;
+	idComplex dx, cx, b, d, f, g, s, gps, gms, g2;
+
+	for ( i = 1; i <= MAX_ITERATIONS; i++ ) {
+		b = coef[degree];
+		err = b.Abs();
+		d.Zero();
+		f.Zero();
+		abx = x.Abs();
+		for ( j = degree - 1; j >= 0; j-- ) {
+			f = x * f + d;
+			d = x * d + b;
+			b = x * b + coef[j];
+			err = b.Abs() + abx * err;
+		}
+		if ( b.Abs() < err * EPSILON ) {
+			return i;
+		}
+		g = d / b;
+		g2 = g * g;
+		s = ( ( degree - 1 ) * ( degree * ( g2 - 2.0f * f / b ) - g2 ) ).Sqrt();
+		gps = g + s;
+		gms = g - s;
+		abp = gps.Abs();
+		abm = gms.Abs();
+		if ( abp < abm ) {
+			gps = gms;
+		}
+		if ( Max( abp, abm ) > 0.0f ) {
+			dx = degree / gps;
+		} else {
+			dx = idMath::Exp( idMath::Log( 1.0f + abx ) ) * idComplex( idMath::Cos( i ), idMath::Sin( i ) );
+		}
+		cx = x - dx;
+		if ( x == cx ) {
+			return i;
+		}
+		if ( i % MT == 0 ) {
+			x = cx;
+		} else {
+			x -= frac[i/MT] * dx;
+		}
+	}
+	return i;
+}
+
+/*
+=============
+idPolynomial::GetRoots
+=============
+*/
+int idPolynomial::GetRoots( idComplex *roots ) const {
+	int i, j;
+	idComplex x, b, c, *coef;
+
+	coef = (idComplex *) _alloca16( ( degree + 1 ) * sizeof( idComplex ) );
+	for ( i = 0; i <= degree; i++ ) {
+		coef[i].Set( coefficient[i], 0.0f );
+	}
+
+	for ( i = degree - 1; i >= 0; i-- ) {
+		x.Zero();
+		Laguer( coef, i + 1, x );
+		if ( idMath::Fabs( x.i ) < 2.0f * EPSILON * idMath::Fabs( x.r ) ) {
+			x.i = 0.0f;
+		}
+		roots[i] = x;
+		b = coef[i+1];
+		for ( j = i; j >= 0; j-- ) {
+			c = coef[j];
+			coef[j] = b;
+			b = x * b + c;
+		}
+	}
+
+	for ( i = 0; i <= degree; i++ ) {
+		coef[i].Set( coefficient[i], 0.0f );
+	}
+	for ( i = 0; i < degree; i++ ) {
+		Laguer( coef, degree, roots[i] );
+	}
+
+	for ( i = 1; i < degree; i++ ) {
+		x = roots[i];
+		for ( j = i - 1; j >= 0; j-- ) {
+			if ( roots[j].r <= x.r ) {
+				break;
+			}
+			roots[j+1] = roots[j];
+		}
+		roots[j+1] = x;
+	}
+
+	return degree;
+}
+
+/*
+=============
+idPolynomial::GetRoots
+=============
+*/
+int idPolynomial::GetRoots( float *roots ) const {
+	int i, num;
+	idComplex *complexRoots;
+
+	switch( degree ) {
+		case 0: return 0;
+		case 1: return GetRoots1( coefficient[1], coefficient[0], roots );
+		case 2: return GetRoots2( coefficient[2], coefficient[1], coefficient[0], roots );
+		case 3: return GetRoots3( coefficient[3], coefficient[2], coefficient[1], coefficient[0], roots );
+		case 4: return GetRoots4( coefficient[4], coefficient[3], coefficient[2], coefficient[1], coefficient[0], roots );
+	}
+
+	// The Abel-Ruffini theorem states that there is no general solution
+	// in radicals to polynomial equations of degree five or higher.
+	// A polynomial equation can be solved by radicals if and only if
+	// its Galois group is a solvable group.
+
+	complexRoots = (idComplex *) _alloca16( degree * sizeof( idComplex ) );
+
+	GetRoots( complexRoots );
+
+	for ( num = i = 0; i < degree; i++ ) {
+		if ( complexRoots[i].i == 0.0f ) {
+			roots[i] = complexRoots[i].r;
+			num++;
+		}
+	}
+	return num;
+}
+
+/*
+=============
+idPolynomial::ToString
+=============
+*/
+const char *idPolynomial::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=============
+idPolynomial::Test
+=============
+*/
+void idPolynomial::Test( void ) {
+	int i, num;
+	float roots[4], value;
+	idComplex complexRoots[4], complexValue;
+	idPolynomial p;
+
+	p = idPolynomial( -5.0f, 4.0f );
+	num = p.GetRoots( roots );
+	for ( i = 0; i < num; i++ ) {
+		value = p.GetValue( roots[i] );
+		assert( idMath::Fabs( value ) < 1e-4f );
+	}
+
+	p = idPolynomial( -5.0f, 4.0f, 3.0f );
+	num = p.GetRoots( roots );
+	for ( i = 0; i < num; i++ ) {
+		value = p.GetValue( roots[i] );
+		assert( idMath::Fabs( value ) < 1e-4f );
+	}
+
+	p = idPolynomial( 1.0f, 4.0f, 3.0f, -2.0f );
+	num = p.GetRoots( roots );
+	for ( i = 0; i < num; i++ ) {
+		value = p.GetValue( roots[i] );
+		assert( idMath::Fabs( value ) < 1e-4f );
+	}
+
+	p = idPolynomial( 5.0f, 4.0f, 3.0f, -2.0f );
+	num = p.GetRoots( roots );
+	for ( i = 0; i < num; i++ ) {
+		value = p.GetValue( roots[i] );
+		assert( idMath::Fabs( value ) < 1e-4f );
+	}
+
+	p = idPolynomial( -5.0f, 4.0f, 3.0f, 2.0f, 1.0f );
+	num = p.GetRoots( roots );
+	for ( i = 0; i < num; i++ ) {
+		value = p.GetValue( roots[i] );
+		assert( idMath::Fabs( value ) < 1e-4f );
+	}
+
+	p = idPolynomial( 1.0f, 4.0f, 3.0f, -2.0f );
+	num = p.GetRoots( complexRoots );
+	for ( i = 0; i < num; i++ ) {
+		complexValue = p.GetValue( complexRoots[i] );
+		assert( idMath::Fabs( complexValue.r ) < 1e-4f && idMath::Fabs( complexValue.i ) < 1e-4f );
+	}
+
+	p = idPolynomial( 5.0f, 4.0f, 3.0f, -2.0f );
+	num = p.GetRoots( complexRoots );
+	for ( i = 0; i < num; i++ ) {
+		complexValue = p.GetValue( complexRoots[i] );
+		assert( idMath::Fabs( complexValue.r ) < 1e-4f && idMath::Fabs( complexValue.i ) < 1e-4f );
+	}
+}

neo/idlib/math/Polynomial.h

@@ -0,0 +1,629 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_POLYNOMIAL_H__
+#define __MATH_POLYNOMIAL_H__
+
+/*
+===============================================================================
+
+	Polynomial of arbitrary degree with real coefficients.
+
+===============================================================================
+*/
+
+
+class idPolynomial {
+public:
+					idPolynomial( void );
+					explicit idPolynomial( int d );
+					explicit idPolynomial( float a, float b );
+					explicit idPolynomial( float a, float b, float c );
+					explicit idPolynomial( float a, float b, float c, float d );
+					explicit idPolynomial( float a, float b, float c, float d, float e );
+
+	float			operator[]( int index ) const;
+	float &			operator[]( int index );
+
+	idPolynomial	operator-() const;
+	idPolynomial &	operator=( const idPolynomial &p );
+
+	idPolynomial	operator+( const idPolynomial &p ) const;
+	idPolynomial	operator-( const idPolynomial &p ) const;
+	idPolynomial	operator*( const float s ) const;
+	idPolynomial	operator/( const float s ) const;
+
+	idPolynomial &	operator+=( const idPolynomial &p );
+	idPolynomial &	operator-=( const idPolynomial &p );
+	idPolynomial &	operator*=( const float s );
+	idPolynomial &	operator/=( const float s );
+
+	bool			Compare( const idPolynomial &p ) const;						// exact compare, no epsilon
+	bool			Compare( const idPolynomial &p, const float epsilon ) const;// compare with epsilon
+	bool			operator==(	const idPolynomial &p ) const;					// exact compare, no epsilon
+	bool			operator!=(	const idPolynomial &p ) const;					// exact compare, no epsilon
+
+	void			Zero( void );
+	void			Zero( int d );
+
+	int				GetDimension( void ) const;									// get the degree of the polynomial
+	int				GetDegree( void ) const;									// get the degree of the polynomial
+	float			GetValue( const float x ) const;							// evaluate the polynomial with the given real value
+	idComplex		GetValue( const idComplex &x ) const;						// evaluate the polynomial with the given complex value
+	idPolynomial	GetDerivative( void ) const;								// get the first derivative of the polynomial
+	idPolynomial	GetAntiDerivative( void ) const;							// get the anti derivative of the polynomial
+
+	int				GetRoots( idComplex *roots ) const;							// get all roots
+	int				GetRoots( float *roots ) const;								// get the real roots
+
+	static int		GetRoots1( float a, float b, float *roots );
+	static int		GetRoots2( float a, float b, float c, float *roots );
+	static int		GetRoots3( float a, float b, float c, float d, float *roots );
+	static int		GetRoots4( float a, float b, float c, float d, float e, float *roots );
+
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+
+	static void		Test( void );
+
+private:
+	int				degree;
+	int				allocated;
+	float *			coefficient;
+
+	void			Resize( int d, bool keep );
+	int				Laguer( const idComplex *coef, const int degree, idComplex &r ) const;
+};
+
+ID_INLINE idPolynomial::idPolynomial( void ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+}
+
+ID_INLINE idPolynomial::idPolynomial( int d ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+	Resize( d, false );
+}
+
+ID_INLINE idPolynomial::idPolynomial( float a, float b ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+	Resize( 1, false );
+	coefficient[0] = b;
+	coefficient[1] = a;
+}
+
+ID_INLINE idPolynomial::idPolynomial( float a, float b, float c ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+	Resize( 2, false );
+	coefficient[0] = c;
+	coefficient[1] = b;
+	coefficient[2] = a;
+}
+
+ID_INLINE idPolynomial::idPolynomial( float a, float b, float c, float d ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+	Resize( 3, false );
+	coefficient[0] = d;
+	coefficient[1] = c;
+	coefficient[2] = b;
+	coefficient[3] = a;
+}
+
+ID_INLINE idPolynomial::idPolynomial( float a, float b, float c, float d, float e ) {
+	degree = -1;
+	allocated = 0;
+	coefficient = NULL;
+	Resize( 4, false );
+	coefficient[0] = e;
+	coefficient[1] = d;
+	coefficient[2] = c;
+	coefficient[3] = b;
+	coefficient[4] = a;
+}
+
+ID_INLINE float idPolynomial::operator[]( int index ) const {
+	assert( index >= 0 && index <= degree );
+	return coefficient[ index ];
+}
+
+ID_INLINE float& idPolynomial::operator[]( int index ) {
+	assert( index >= 0 && index <= degree );
+	return coefficient[ index ];
+}
+
+ID_INLINE idPolynomial idPolynomial::operator-() const {
+	int i;
+	idPolynomial n;
+
+	n = *this;
+	for ( i = 0; i <= degree; i++ ) {
+		n[i] = -n[i];
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial &idPolynomial::operator=( const idPolynomial &p ) { 
+	Resize( p.degree, false );
+	for ( int i = 0; i <= degree; i++ ) {
+		coefficient[i] = p.coefficient[i];
+	}
+	return *this;
+}
+
+ID_INLINE idPolynomial idPolynomial::operator+( const idPolynomial &p ) const {
+	int i;
+	idPolynomial n;
+
+	if ( degree > p.degree ) {
+		n.Resize( degree, false );
+		for ( i = 0; i <= p.degree; i++ ) {
+			n.coefficient[i] = coefficient[i] + p.coefficient[i];
+		}
+		for ( ; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i];
+		}
+		n.degree = degree;
+	} else if ( p.degree > degree ) {
+		n.Resize( p.degree, false );
+		for ( i = 0; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i] + p.coefficient[i];
+		}
+		for ( ; i <= p.degree; i++ ) {
+			n.coefficient[i] = p.coefficient[i];
+		}
+		n.degree = p.degree;
+	} else {
+		n.Resize( degree, false );
+		n.degree = 0;
+		for ( i = 0; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i] + p.coefficient[i];
+			if ( n.coefficient[i] != 0.0f ) {
+				n.degree = i;
+			}
+		}
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial idPolynomial::operator-( const idPolynomial &p ) const {
+	int i;
+	idPolynomial n;
+
+	if ( degree > p.degree ) {
+		n.Resize( degree, false );
+		for ( i = 0; i <= p.degree; i++ ) {
+			n.coefficient[i] = coefficient[i] - p.coefficient[i];
+		}
+		for ( ; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i];
+		}
+		n.degree = degree;
+	} else if ( p.degree >= degree ) {
+		n.Resize( p.degree, false );
+		for ( i = 0; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i] - p.coefficient[i];
+		}
+		for ( ; i <= p.degree; i++ ) {
+			n.coefficient[i] = - p.coefficient[i];
+		}
+		n.degree = p.degree;
+	} else {
+		n.Resize( degree, false );
+		n.degree = 0;
+		for ( i = 0; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i] - p.coefficient[i];
+			if ( n.coefficient[i] != 0.0f ) {
+				n.degree = i;
+			}
+		}
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial idPolynomial::operator*( const float s ) const {
+	idPolynomial n;
+
+	if ( s == 0.0f ) {
+		n.degree = 0;
+	} else {
+		n.Resize( degree, false );
+		for ( int i = 0; i <= degree; i++ ) {
+			n.coefficient[i] = coefficient[i] * s;
+		}
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial idPolynomial::operator/( const float s ) const {
+	float invs;
+	idPolynomial n;
+
+	assert( s != 0.0f );
+	n.Resize( degree, false );
+	invs = 1.0f / s;
+	for ( int i = 0; i <= degree; i++ ) {
+		n.coefficient[i] = coefficient[i] * invs;
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial &idPolynomial::operator+=( const idPolynomial &p ) {
+	int i;
+
+	if ( degree > p.degree ) {
+		for ( i = 0; i <= p.degree; i++ ) {
+			coefficient[i] += p.coefficient[i];
+		}
+	} else if ( p.degree > degree ) {
+		Resize( p.degree, true );
+		for ( i = 0; i <= degree; i++ ) {
+			coefficient[i] += p.coefficient[i];
+		}
+		for ( ; i <= p.degree; i++ ) {
+			coefficient[i] = p.coefficient[i];
+		}
+	} else {
+		for ( i = 0; i <= degree; i++ ) {
+			coefficient[i] += p.coefficient[i];
+			if ( coefficient[i] != 0.0f ) {
+				degree = i;
+			}
+		}
+	}
+	return *this;
+}
+
+ID_INLINE idPolynomial &idPolynomial::operator-=( const idPolynomial &p ) {
+	int i;
+
+	if ( degree > p.degree ) {
+		for ( i = 0; i <= p.degree; i++ ) {
+			coefficient[i] -= p.coefficient[i];
+		}
+	} else if ( p.degree > degree ) {
+		Resize( p.degree, true );
+		for ( i = 0; i <= degree; i++ ) {
+			coefficient[i] -= p.coefficient[i];
+		}
+		for ( ; i <= p.degree; i++ ) {
+			coefficient[i] = - p.coefficient[i];
+		}
+	} else {
+		for ( i = 0; i <= degree; i++ ) {
+			coefficient[i] -= p.coefficient[i];
+			if ( coefficient[i] != 0.0f ) {
+				degree = i;
+			}
+		}
+	}
+	return *this;
+}
+
+ID_INLINE idPolynomial &idPolynomial::operator*=( const float s ) {
+	if ( s == 0.0f ) {
+		degree = 0;
+	} else {
+		for ( int i = 0; i <= degree; i++ ) {
+			coefficient[i] *= s;
+		}
+	}
+	return *this;
+}
+
+ID_INLINE idPolynomial &idPolynomial::operator/=( const float s ) {
+	float invs;
+
+	assert( s != 0.0f );
+	invs = 1.0f / s;
+	for ( int i = 0; i <= degree; i++ ) {
+		coefficient[i] = invs;
+	}
+	return *this;;
+}
+
+ID_INLINE bool idPolynomial::Compare( const idPolynomial &p ) const {
+	if ( degree != p.degree ) {
+		return false;
+	}
+	for ( int i = 0; i <= degree; i++ ) {
+		if ( coefficient[i] != p.coefficient[i] ) {
+			return false;
+		}
+	}
+	return true;
+}
+
+ID_INLINE bool idPolynomial::Compare( const idPolynomial &p, const float epsilon ) const {
+	if ( degree != p.degree ) {
+		return false;
+	}
+	for ( int i = 0; i <= degree; i++ ) {
+		if ( idMath::Fabs( coefficient[i] - p.coefficient[i] ) > epsilon ) {
+			return false;
+		}
+	}
+	return true;
+}
+
+ID_INLINE bool idPolynomial::operator==( const idPolynomial &p ) const {
+	return Compare( p );
+}
+
+ID_INLINE bool idPolynomial::operator!=( const idPolynomial &p ) const {
+	return !Compare( p );
+}
+
+ID_INLINE void idPolynomial::Zero( void ) {
+	degree = 0;
+}
+
+ID_INLINE void idPolynomial::Zero( int d ) {
+	Resize( d, false );
+	for ( int i = 0; i <= degree; i++ ) {
+		coefficient[i] = 0.0f;
+	}
+}
+
+ID_INLINE int idPolynomial::GetDimension( void ) const {
+	return degree;
+}
+
+ID_INLINE int idPolynomial::GetDegree( void ) const {
+	return degree;
+}
+
+ID_INLINE float idPolynomial::GetValue( const float x ) const {
+	float y, z;
+	y = coefficient[0];
+	z = x;
+	for ( int i = 1; i <= degree; i++ ) {
+		y += coefficient[i] * z;
+		z *= x;
+	}
+	return y;
+}
+
+ID_INLINE idComplex idPolynomial::GetValue( const idComplex &x ) const {
+	idComplex y, z;
+	y.Set( coefficient[0], 0.0f );
+	z = x;
+	for ( int i = 1; i <= degree; i++ ) {
+		y += coefficient[i] * z;
+		z *= x;
+	}
+	return y;
+}
+
+ID_INLINE idPolynomial idPolynomial::GetDerivative( void ) const {
+	idPolynomial n;
+
+	if ( degree == 0 ) {
+		return n;
+	}
+	n.Resize( degree - 1, false );
+	for ( int i = 1; i <= degree; i++ ) {
+		n.coefficient[i-1] = i * coefficient[i];
+	}
+	return n;
+}
+
+ID_INLINE idPolynomial idPolynomial::GetAntiDerivative( void ) const {
+	idPolynomial n;
+
+	if ( degree == 0 ) {
+		return n;
+	}
+	n.Resize( degree + 1, false );
+	n.coefficient[0] = 0.0f;
+	for ( int i = 0; i <= degree; i++ ) {
+		n.coefficient[i+1] = coefficient[i] / ( i + 1 );
+	}
+	return n;
+}
+
+ID_INLINE int idPolynomial::GetRoots1( float a, float b, float *roots ) {
+	assert( a != 0.0f );
+	roots[0] = - b / a;
+	return 1;
+}
+
+ID_INLINE int idPolynomial::GetRoots2( float a, float b, float c, float *roots ) {
+	float inva, ds;
+
+	if ( a != 1.0f ) {
+		assert( a != 0.0f );
+		inva = 1.0f / a;
+		c *= inva;
+		b *= inva;
+	}
+	ds = b * b - 4.0f * c;
+	if ( ds < 0.0f ) {
+		return 0;
+	} else if ( ds > 0.0f ) {
+		ds = idMath::Sqrt( ds );
+		roots[0] = 0.5f * ( -b - ds );
+		roots[1] = 0.5f * ( -b + ds );
+		return 2;
+	} else {
+		roots[0] = 0.5f * -b;
+		return 1;
+	}
+}
+
+ID_INLINE int idPolynomial::GetRoots3( float a, float b, float c, float d, float *roots ) {
+	float inva, f, g, halfg, ofs, ds, dist, angle, cs, ss, t;
+
+	if ( a != 1.0f ) {
+		assert( a != 0.0f );
+		inva = 1.0f / a;
+		d *= inva;
+		c *= inva;
+		b *= inva;
+	}
+
+	f = ( 1.0f / 3.0f ) * ( 3.0f * c - b * b );
+	g = ( 1.0f / 27.0f ) * ( 2.0f * b * b * b - 9.0f * c * b + 27.0f * d );
+	halfg = 0.5f * g;
+	ofs = ( 1.0f / 3.0f ) * b;
+	ds = 0.25f * g * g + ( 1.0f / 27.0f ) * f * f * f;
+
+	if ( ds < 0.0f ) {
+		dist = idMath::Sqrt( ( -1.0f / 3.0f ) * f );
+		angle = ( 1.0f / 3.0f ) * idMath::ATan( idMath::Sqrt( -ds ), -halfg );
+		cs = idMath::Cos( angle );
+		ss = idMath::Sin( angle );
+		roots[0] = 2.0f * dist * cs - ofs;
+		roots[1] = -dist * ( cs + idMath::SQRT_THREE * ss ) - ofs;
+		roots[2] = -dist * ( cs - idMath::SQRT_THREE * ss ) - ofs;
+		return 3;
+	} else if ( ds > 0.0f )  {
+		ds = idMath::Sqrt( ds );
+		t = -halfg + ds;
+		if ( t >= 0.0f ) {
+			roots[0] = idMath::Pow( t, ( 1.0f / 3.0f ) );
+		} else {
+			roots[0] = -idMath::Pow( -t, ( 1.0f / 3.0f ) );
+		}
+		t = -halfg - ds;
+		if ( t >= 0.0f ) {
+			roots[0] += idMath::Pow( t, ( 1.0f / 3.0f ) );
+		} else {
+			roots[0] -= idMath::Pow( -t, ( 1.0f / 3.0f ) );
+		}
+		roots[0] -= ofs;
+		return 1;
+	} else {
+		if ( halfg >= 0.0f ) {
+			t = -idMath::Pow( halfg, ( 1.0f / 3.0f ) );
+		} else {
+			t = idMath::Pow( -halfg, ( 1.0f / 3.0f ) );
+		}
+		roots[0] = 2.0f * t - ofs;
+		roots[1] = -t - ofs;
+		roots[2] = roots[1];
+		return 3;
+	}
+}
+
+ID_INLINE int idPolynomial::GetRoots4( float a, float b, float c, float d, float e, float *roots ) {
+	int count;
+	float inva, y, ds, r, s1, s2, t1, t2, tp, tm;
+	float roots3[3];
+
+	if ( a != 1.0f ) {
+		assert( a != 0.0f );
+		inva = 1.0f / a;
+		e *= inva;
+		d *= inva;
+		c *= inva;
+		b *= inva;
+	}
+
+	count = 0;
+
+	GetRoots3( 1.0f, -c, b * d - 4.0f * e, -b * b * e + 4.0f * c * e - d * d, roots3 );
+	y = roots3[0];
+	ds = 0.25f * b * b - c + y;
+
+	if ( ds < 0.0f ) {
+		return 0;
+	} else if ( ds > 0.0f ) {
+		r = idMath::Sqrt( ds );
+		t1 = 0.75f * b * b - r * r - 2.0f * c;
+		t2 = ( 4.0f * b * c - 8.0f * d - b * b * b ) / ( 4.0f * r );
+		tp = t1 + t2;
+		tm = t1 - t2;
+
+		if ( tp >= 0.0f ) {
+			s1 = idMath::Sqrt( tp );
+			roots[count++] = -0.25f * b + 0.5f * ( r + s1 );
+			roots[count++] = -0.25f * b + 0.5f * ( r - s1 );
+		}
+		if ( tm >= 0.0f ) {
+			s2 = idMath::Sqrt( tm );
+			roots[count++] = -0.25f * b + 0.5f * ( s2 - r );
+			roots[count++] = -0.25f * b - 0.5f * ( s2 + r );
+		}
+		return count;
+	} else {
+		t2 = y * y - 4.0f * e;
+		if ( t2 >= 0.0f ) {
+			t2 = 2.0f * idMath::Sqrt( t2 );
+			t1 = 0.75f * b * b - 2.0f * c;
+			if ( t1 + t2 >= 0.0f ) {
+				s1 = idMath::Sqrt( t1 + t2 );
+				roots[count++] = -0.25f * b + 0.5f * s1;
+				roots[count++] = -0.25f * b - 0.5f * s1;
+			}
+			if ( t1 - t2 >= 0.0f ) {
+				s2 = idMath::Sqrt( t1 - t2 );
+				roots[count++] = -0.25f * b + 0.5f * s2;
+				roots[count++] = -0.25f * b - 0.5f * s2;
+			}
+		}
+		return count;
+	}
+}
+
+ID_INLINE const float *idPolynomial::ToFloatPtr( void ) const {
+	return coefficient;
+}
+
+ID_INLINE float *idPolynomial::ToFloatPtr( void ) {
+	return coefficient;
+}
+
+ID_INLINE void idPolynomial::Resize( int d, bool keep ) {
+	int alloc = ( d + 1 + 3 ) & ~3;
+	if ( alloc > allocated ) {
+		float *ptr = (float *) Mem_Alloc16( alloc * sizeof( float ) );
+		if ( coefficient != NULL ) {
+			if ( keep ) {
+				for ( int i = 0; i <= degree; i++ ) {
+					ptr[i] = coefficient[i];
+				}
+			}
+			Mem_Free16( coefficient );
+		}
+		allocated = alloc;
+		coefficient = ptr;
+	}
+	degree = d;
+}
+
+#endif /* !__MATH_POLYNOMIAL_H__ */

neo/idlib/math/Quat.cpp

@@ -0,0 +1,252 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+/*
+=====================
+idQuat::ToAngles
+=====================
+*/
+idAngles idQuat::ToAngles( void ) const {
+	return ToMat3().ToAngles();
+}
+
+/*
+=====================
+idQuat::ToRotation
+=====================
+*/
+idRotation idQuat::ToRotation( void ) const {
+	idVec3 vec;
+	float angle;
+
+	vec.x = x;
+	vec.y = y;
+	vec.z = z;
+	angle = idMath::ACos( w );
+	if ( angle == 0.0f ) {
+		vec.Set( 0.0f, 0.0f, 1.0f );
+	} else {
+		//vec *= (1.0f / sin( angle ));
+		vec.Normalize();
+		vec.FixDegenerateNormal();
+		angle *= 2.0f * idMath::M_RAD2DEG;
+	}
+	return idRotation( vec3_origin, vec, angle );
+}
+
+/*
+=====================
+idQuat::ToMat3
+=====================
+*/
+idMat3 idQuat::ToMat3( void ) const {
+	idMat3	mat;
+	float	wx, wy, wz;
+	float	xx, yy, yz;
+	float	xy, xz, zz;
+	float	x2, y2, z2;
+
+	x2 = x + x;
+	y2 = y + y;
+	z2 = z + z;
+
+	xx = x * x2;
+	xy = x * y2;
+	xz = x * z2;
+
+	yy = y * y2;
+	yz = y * z2;
+	zz = z * z2;
+
+	wx = w * x2;
+	wy = w * y2;
+	wz = w * z2;
+
+	mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
+	mat[ 0 ][ 1 ] = xy - wz;
+	mat[ 0 ][ 2 ] = xz + wy;
+
+	mat[ 1 ][ 0 ] = xy + wz;
+	mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
+	mat[ 1 ][ 2 ] = yz - wx;
+
+	mat[ 2 ][ 0 ] = xz - wy;
+	mat[ 2 ][ 1 ] = yz + wx;
+	mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
+
+	return mat;
+}
+
+/*
+=====================
+idQuat::ToMat4
+=====================
+*/
+idMat4 idQuat::ToMat4( void ) const {
+	return ToMat3().ToMat4();
+}
+
+/*
+=====================
+idQuat::ToCQuat
+=====================
+*/
+idCQuat idQuat::ToCQuat( void ) const {
+	if ( w < 0.0f ) {
+		return idCQuat( -x, -y, -z );
+	}
+	return idCQuat( x, y, z );
+}
+
+/*
+============
+idQuat::ToAngularVelocity
+============
+*/
+idVec3 idQuat::ToAngularVelocity( void ) const {
+	idVec3 vec;
+
+	vec.x = x;
+	vec.y = y;
+	vec.z = z;
+	vec.Normalize();
+	return vec * idMath::ACos( w );
+}
+
+/*
+=============
+idQuat::ToString
+=============
+*/
+const char *idQuat::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=====================
+idQuat::Slerp
+
+Spherical linear interpolation between two quaternions.
+=====================
+*/
+idQuat &idQuat::Slerp( const idQuat &from, const idQuat &to, float t ) {
+	idQuat	temp;
+	float	omega, cosom, sinom, scale0, scale1;
+
+	if ( t <= 0.0f ) {
+		*this = from;
+		return *this;
+	}
+
+	if ( t >= 1.0f ) {
+		*this = to;
+		return *this;
+	}
+
+	if ( from == to ) {
+		*this = to;
+		return *this;
+	}
+
+	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
+	if ( cosom < 0.0f ) {
+		temp = -to;
+		cosom = -cosom;
+	} else {
+		temp = to;
+	}
+
+	if ( ( 1.0f - cosom ) > 1e-6f ) {
+#if 0
+		omega = acos( cosom );
+		sinom = 1.0f / sin( omega );
+		scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
+		scale1 = sin( t * omega ) * sinom;
+#else
+		scale0 = 1.0f - cosom * cosom;
+		sinom = idMath::InvSqrt( scale0 );
+		omega = idMath::ATan16( scale0 * sinom, cosom );
+		scale0 = idMath::Sin16( ( 1.0f - t ) * omega ) * sinom;
+		scale1 = idMath::Sin16( t * omega ) * sinom;
+#endif
+	} else {
+		scale0 = 1.0f - t;
+		scale1 = t;
+	}
+
+	*this = ( scale0 * from ) + ( scale1 * temp );
+	return *this;
+}
+
+/*
+=============
+idCQuat::ToAngles
+=============
+*/
+idAngles idCQuat::ToAngles( void ) const {
+	return ToQuat().ToAngles();
+}
+
+/*
+=============
+idCQuat::ToRotation
+=============
+*/
+idRotation idCQuat::ToRotation( void ) const {
+	return ToQuat().ToRotation();
+}
+
+/*
+=============
+idCQuat::ToMat3
+=============
+*/
+idMat3 idCQuat::ToMat3( void ) const {
+	return ToQuat().ToMat3();
+}
+
+/*
+=============
+idCQuat::ToMat4
+=============
+*/
+idMat4 idCQuat::ToMat4( void ) const {
+	return ToQuat().ToMat4();
+}
+
+/*
+=============
+idCQuat::ToString
+=============
+*/
+const char *idCQuat::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}

neo/idlib/math/Quat.h

@@ -0,0 +1,404 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_QUAT_H__
+#define __MATH_QUAT_H__
+
+/*
+===============================================================================
+
+	Quaternion
+
+===============================================================================
+*/
+
+
+class idVec3;
+class idAngles;
+class idRotation;
+class idMat3;
+class idMat4;
+class idCQuat;
+
+class idQuat {
+public:
+	float			x;
+	float			y;
+	float			z;
+	float			w;
+
+					idQuat( void );
+					idQuat( float x, float y, float z, float w );
+
+	void 			Set( float x, float y, float z, float w );
+
+	float			operator[]( int index ) const;
+	float &			operator[]( int index );
+	idQuat			operator-() const;
+	idQuat &		operator=( const idQuat &a );
+	idQuat			operator+( const idQuat &a ) const;
+	idQuat &		operator+=( const idQuat &a );
+	idQuat			operator-( const idQuat &a ) const;
+	idQuat &		operator-=( const idQuat &a );
+	idQuat			operator*( const idQuat &a ) const;
+	idVec3			operator*( const idVec3 &a ) const;
+	idQuat			operator*( float a ) const;
+	idQuat &		operator*=( const idQuat &a );
+	idQuat &		operator*=( float a );
+
+	friend idQuat	operator*( const float a, const idQuat &b );
+	friend idVec3	operator*( const idVec3 &a, const idQuat &b );
+
+	bool			Compare( const idQuat &a ) const;						// exact compare, no epsilon
+	bool			Compare( const idQuat &a, const float epsilon ) const;	// compare with epsilon
+	bool			operator==(	const idQuat &a ) const;					// exact compare, no epsilon
+	bool			operator!=(	const idQuat &a ) const;					// exact compare, no epsilon
+
+	idQuat			Inverse( void ) const;
+	float			Length( void ) const;
+	idQuat &		Normalize( void );
+
+	float			CalcW( void ) const;
+	int				GetDimension( void ) const;
+
+	idAngles		ToAngles( void ) const;
+	idRotation		ToRotation( void ) const;
+	idMat3			ToMat3( void ) const;
+	idMat4			ToMat4( void ) const;
+	idCQuat			ToCQuat( void ) const;
+	idVec3			ToAngularVelocity( void ) const;
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+
+	idQuat &		Slerp( const idQuat &from, const idQuat &to, float t );
+};
+
+ID_INLINE idQuat::idQuat( void ) {
+}
+
+ID_INLINE idQuat::idQuat( float x, float y, float z, float w ) {
+	this->x = x;
+	this->y = y;
+	this->z = z;
+	this->w = w;
+}
+
+ID_INLINE float idQuat::operator[]( int index ) const {
+	assert( ( index >= 0 ) && ( index < 4 ) );
+	return ( &x )[ index ];
+}
+
+ID_INLINE float& idQuat::operator[]( int index ) {
+	assert( ( index >= 0 ) && ( index < 4 ) );
+	return ( &x )[ index ];
+}
+
+ID_INLINE idQuat idQuat::operator-() const {
+	return idQuat( -x, -y, -z, -w );
+}
+
+ID_INLINE idQuat &idQuat::operator=( const idQuat &a ) {
+	x = a.x;
+	y = a.y;
+	z = a.z;
+	w = a.w;
+
+	return *this;
+}
+
+ID_INLINE idQuat idQuat::operator+( const idQuat &a ) const {
+	return idQuat( x + a.x, y + a.y, z + a.z, w + a.w );
+}
+
+ID_INLINE idQuat& idQuat::operator+=( const idQuat &a ) {
+	x += a.x;
+	y += a.y;
+	z += a.z;
+	w += a.w;
+
+	return *this;
+}
+
+ID_INLINE idQuat idQuat::operator-( const idQuat &a ) const {
+	return idQuat( x - a.x, y - a.y, z - a.z, w - a.w );
+}
+
+ID_INLINE idQuat& idQuat::operator-=( const idQuat &a ) {
+	x -= a.x;
+	y -= a.y;
+	z -= a.z;
+	w -= a.w;
+
+	return *this;
+}
+
+ID_INLINE idQuat idQuat::operator*( const idQuat &a ) const {
+	return idQuat(	w*a.x + x*a.w + y*a.z - z*a.y,
+					w*a.y + y*a.w + z*a.x - x*a.z,
+					w*a.z + z*a.w + x*a.y - y*a.x,
+					w*a.w - x*a.x - y*a.y - z*a.z );
+}
+
+ID_INLINE idVec3 idQuat::operator*( const idVec3 &a ) const {
+#if 0
+	// it's faster to do the conversion to a 3x3 matrix and multiply the vector by this 3x3 matrix
+	return ( ToMat3() * a );
+#else
+	// result = this->Inverse() * idQuat( a.x, a.y, a.z, 0.0f ) * (*this)
+	float xxzz = x*x - z*z;
+	float wwyy = w*w - y*y;
+
+	float xw2 = x*w*2.0f;
+	float xy2 = x*y*2.0f;
+	float xz2 = x*z*2.0f;
+	float yw2 = y*w*2.0f;
+	float yz2 = y*z*2.0f;
+	float zw2 = z*w*2.0f;
+
+	return idVec3(
+		(xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
+		(xy2 - zw2)*a.x			+ (y*y+w*w-x*x-z*z)*a.y	+ (yz2 + xw2)*a.z,
+		(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
+	);
+#endif
+}
+
+ID_INLINE idQuat idQuat::operator*( float a ) const {
+	return idQuat( x * a, y * a, z * a, w * a );
+}
+
+ID_INLINE idQuat operator*( const float a, const idQuat &b ) {
+	return b * a;
+}
+
+ID_INLINE idVec3 operator*( const idVec3 &a, const idQuat &b ) {
+	return b * a;
+}
+
+ID_INLINE idQuat& idQuat::operator*=( const idQuat &a ) {
+	*this = *this * a;
+
+	return *this;
+}
+
+ID_INLINE idQuat& idQuat::operator*=( float a ) {
+	x *= a;
+	y *= a;
+	z *= a;
+	w *= a;
+
+	return *this;
+}
+
+ID_INLINE bool idQuat::Compare( const idQuat &a ) const {
+	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
+}
+
+ID_INLINE bool idQuat::Compare( const idQuat &a, const float epsilon ) const {
+	if ( idMath::Fabs( x - a.x ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( y - a.y ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( z - a.z ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( w - a.w ) > epsilon ) {
+		return false;
+	}
+	return true;
+}
+
+ID_INLINE bool idQuat::operator==( const idQuat &a ) const {
+	return Compare( a );
+}
+
+ID_INLINE bool idQuat::operator!=( const idQuat &a ) const {
+	return !Compare( a );
+}
+
+ID_INLINE void idQuat::Set( float x, float y, float z, float w ) {
+	this->x = x;
+	this->y = y;
+	this->z = z;
+	this->w = w;
+}
+
+ID_INLINE idQuat idQuat::Inverse( void ) const {
+	return idQuat( -x, -y, -z, w );
+}
+
+ID_INLINE float idQuat::Length( void ) const {
+	float len;
+
+	len = x * x + y * y + z * z + w * w;
+	return idMath::Sqrt( len );
+}
+
+ID_INLINE idQuat& idQuat::Normalize( void ) {
+	float len;
+	float ilength;
+
+	len = this->Length();
+	if ( len ) {
+		ilength = 1 / len;
+		x *= ilength;
+		y *= ilength;
+		z *= ilength;
+		w *= ilength;
+	}
+	return *this;
+}
+
+ID_INLINE float idQuat::CalcW( void ) const {
+	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
+	return sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) );
+}
+
+ID_INLINE int idQuat::GetDimension( void ) const {
+	return 4;
+}
+
+ID_INLINE const float *idQuat::ToFloatPtr( void ) const {
+	return &x;
+}
+
+ID_INLINE float *idQuat::ToFloatPtr( void ) {
+	return &x;
+}
+
+
+/*
+===============================================================================
+
+	Compressed quaternion
+
+===============================================================================
+*/
+
+class idCQuat {
+public:
+	float			x;
+	float			y;
+	float			z;
+
+					idCQuat( void );
+					idCQuat( float x, float y, float z );
+
+	void 			Set( float x, float y, float z );
+
+	float			operator[]( int index ) const;
+	float &			operator[]( int index );
+
+	bool			Compare( const idCQuat &a ) const;						// exact compare, no epsilon
+	bool			Compare( const idCQuat &a, const float epsilon ) const;	// compare with epsilon
+	bool			operator==(	const idCQuat &a ) const;					// exact compare, no epsilon
+	bool			operator!=(	const idCQuat &a ) const;					// exact compare, no epsilon
+
+	int				GetDimension( void ) const;
+
+	idAngles		ToAngles( void ) const;
+	idRotation		ToRotation( void ) const;
+	idMat3			ToMat3( void ) const;
+	idMat4			ToMat4( void ) const;
+	idQuat			ToQuat( void ) const;
+	const float *	ToFloatPtr( void ) const;
+	float *			ToFloatPtr( void );
+	const char *	ToString( int precision = 2 ) const;
+};
+
+ID_INLINE idCQuat::idCQuat( void ) {
+}
+
+ID_INLINE idCQuat::idCQuat( float x, float y, float z ) {
+	this->x = x;
+	this->y = y;
+	this->z = z;
+}
+
+ID_INLINE void idCQuat::Set( float x, float y, float z ) {
+	this->x = x;
+	this->y = y;
+	this->z = z;
+}
+
+ID_INLINE float idCQuat::operator[]( int index ) const {
+	assert( ( index >= 0 ) && ( index < 3 ) );
+	return ( &x )[ index ];
+}
+
+ID_INLINE float& idCQuat::operator[]( int index ) {
+	assert( ( index >= 0 ) && ( index < 3 ) );
+	return ( &x )[ index ];
+}
+
+ID_INLINE bool idCQuat::Compare( const idCQuat &a ) const {
+	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
+}
+
+ID_INLINE bool idCQuat::Compare( const idCQuat &a, const float epsilon ) const {
+	if ( idMath::Fabs( x - a.x ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( y - a.y ) > epsilon ) {
+		return false;
+	}
+	if ( idMath::Fabs( z - a.z ) > epsilon ) {
+		return false;
+	}
+	return true;
+}
+
+ID_INLINE bool idCQuat::operator==( const idCQuat &a ) const {
+	return Compare( a );
+}
+
+ID_INLINE bool idCQuat::operator!=( const idCQuat &a ) const {
+	return !Compare( a );
+}
+
+ID_INLINE int idCQuat::GetDimension( void ) const {
+	return 3;
+}
+
+ID_INLINE idQuat idCQuat::ToQuat( void ) const {
+	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
+	return idQuat( x, y, z, sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) ) );
+}
+
+ID_INLINE const float *idCQuat::ToFloatPtr( void ) const {
+	return &x;
+}
+
+ID_INLINE float *idCQuat::ToFloatPtr( void ) {
+	return &x;
+}
+
+#endif /* !__MATH_QUAT_H__ */

neo/idlib/math/Random.h

@@ -0,0 +1,158 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_RANDOM_H__
+#define __MATH_RANDOM_H__
+
+/*
+===============================================================================
+
+	Random number generator
+
+===============================================================================
+*/
+
+class idRandom {
+public:
+						idRandom( int seed = 0 );
+
+	void				SetSeed( int seed );
+	int					GetSeed( void ) const;
+
+	int					RandomInt( void );			// random integer in the range [0, MAX_RAND]
+	int					RandomInt( int max );		// random integer in the range [0, max[
+	float				RandomFloat( void );		// random number in the range [0.0f, 1.0f]
+	float				CRandomFloat( void );		// random number in the range [-1.0f, 1.0f]
+
+	static const int	MAX_RAND = 0x7fff;
+
+private:
+	int					seed;
+};
+
+ID_INLINE idRandom::idRandom( int seed ) {
+	this->seed = seed;
+}
+
+ID_INLINE void idRandom::SetSeed( int seed ) {
+	this->seed = seed;
+}
+
+ID_INLINE int idRandom::GetSeed( void ) const {
+	return seed;
+}
+
+ID_INLINE int idRandom::RandomInt( void ) {
+	seed = 69069 * seed + 1;
+	return ( seed & idRandom::MAX_RAND );
+}
+
+ID_INLINE int idRandom::RandomInt( int max ) {
+	if ( max == 0 ) {
+		return 0;			// avoid divide by zero error
+	}
+	return RandomInt() % max;
+}
+
+ID_INLINE float idRandom::RandomFloat( void ) {
+	return ( RandomInt() / ( float )( idRandom::MAX_RAND + 1 ) );
+}
+
+ID_INLINE float idRandom::CRandomFloat( void ) {
+	return ( 2.0f * ( RandomFloat() - 0.5f ) );
+}
+
+
+/*
+===============================================================================
+
+	Random number generator
+
+===============================================================================
+*/
+
+class idRandom2 {
+public:
+							idRandom2( unsigned long seed = 0 );
+
+	void					SetSeed( unsigned long seed );
+	unsigned long			GetSeed( void ) const;
+
+	int						RandomInt( void );			// random integer in the range [0, MAX_RAND]
+	int						RandomInt( int max );		// random integer in the range [0, max]
+	float					RandomFloat( void );		// random number in the range [0.0f, 1.0f]
+	float					CRandomFloat( void );		// random number in the range [-1.0f, 1.0f]
+
+	static const int		MAX_RAND = 0x7fff;
+
+private:
+	unsigned long			seed;
+
+	static const unsigned long	IEEE_ONE = 0x3f800000;
+	static const unsigned long	IEEE_MASK = 0x007fffff;
+};
+
+ID_INLINE idRandom2::idRandom2( unsigned long seed ) {
+	this->seed = seed;
+}
+
+ID_INLINE void idRandom2::SetSeed( unsigned long seed ) {
+	this->seed = seed;
+}
+
+ID_INLINE unsigned long idRandom2::GetSeed( void ) const {
+	return seed;
+}
+
+ID_INLINE int idRandom2::RandomInt( void ) {
+	seed = 1664525L * seed + 1013904223L;
+	return ( (int) seed & idRandom2::MAX_RAND );
+}
+
+ID_INLINE int idRandom2::RandomInt( int max ) {
+	if ( max == 0 ) {
+		return 0;		// avoid divide by zero error
+	}
+	return ( RandomInt() >> ( 16 - idMath::BitsForInteger( max ) ) ) % max;
+}
+
+ID_INLINE float idRandom2::RandomFloat( void ) {
+	unsigned long i;
+	seed = 1664525L * seed + 1013904223L;
+	i = idRandom2::IEEE_ONE | ( seed & idRandom2::IEEE_MASK );
+	return ( ( *(float *)&i ) - 1.0f );
+}
+
+ID_INLINE float idRandom2::CRandomFloat( void ) {
+	unsigned long i;
+	seed = 1664525L * seed + 1013904223L;
+	i = idRandom2::IEEE_ONE | ( seed & idRandom2::IEEE_MASK );
+	return ( 2.0f * ( *(float *)&i ) - 3.0f );
+}
+
+#endif /* !__MATH_RANDOM_H__ */

neo/idlib/math/Rotation.cpp

@@ -0,0 +1,157 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+
+/*
+============
+idRotation::ToAngles
+============
+*/
+idAngles idRotation::ToAngles( void ) const {
+	return ToMat3().ToAngles();
+}
+
+/*
+============
+idRotation::ToQuat
+============
+*/
+idQuat idRotation::ToQuat( void ) const {
+	float a, s, c;
+
+	a = angle * ( idMath::M_DEG2RAD * 0.5f );
+	idMath::SinCos( a, s, c );
+	return idQuat( vec.x * s, vec.y * s, vec.z * s, c );
+}
+
+/*
+============
+idRotation::toMat3
+============
+*/
+const idMat3 &idRotation::ToMat3( void ) const {
+	float wx, wy, wz;
+	float xx, yy, yz;
+	float xy, xz, zz;
+	float x2, y2, z2;
+	float a, c, s, x, y, z;
+
+	if ( axisValid ) {
+		return axis;
+	}
+
+	a = angle * ( idMath::M_DEG2RAD * 0.5f );
+	idMath::SinCos( a, s, c );
+
+	x = vec[0] * s;
+	y = vec[1] * s;
+	z = vec[2] * s;
+
+	x2 = x + x;
+	y2 = y + y;
+	z2 = z + z;
+
+	xx = x * x2;
+	xy = x * y2;
+	xz = x * z2;
+
+	yy = y * y2;
+	yz = y * z2;
+	zz = z * z2;
+
+	wx = c * x2;
+	wy = c * y2;
+	wz = c * z2;
+
+	axis[ 0 ][ 0 ] = 1.0f - ( yy + zz );
+	axis[ 0 ][ 1 ] = xy - wz;
+	axis[ 0 ][ 2 ] = xz + wy;
+
+	axis[ 1 ][ 0 ] = xy + wz;
+	axis[ 1 ][ 1 ] = 1.0f - ( xx + zz );
+	axis[ 1 ][ 2 ] = yz - wx;
+
+	axis[ 2 ][ 0 ] = xz - wy;
+	axis[ 2 ][ 1 ] = yz + wx;
+	axis[ 2 ][ 2 ] = 1.0f - ( xx + yy );
+
+	axisValid = true;
+
+	return axis;
+}
+
+/*
+============
+idRotation::ToMat4
+============
+*/
+idMat4 idRotation::ToMat4( void ) const {
+	return ToMat3().ToMat4();
+}
+
+/*
+============
+idRotation::ToAngularVelocity
+============
+*/
+idVec3 idRotation::ToAngularVelocity( void ) const {
+	return vec * DEG2RAD( angle );
+}
+
+/*
+============
+idRotation::Normalize180
+============
+*/
+void idRotation::Normalize180( void ) {
+	angle -= floor( angle / 360.0f ) * 360.0f;
+	if ( angle > 180.0f ) {
+		angle -= 360.0f;
+	}
+	else if ( angle < -180.0f ) {
+		angle += 360.0f;
+	}
+}
+
+/*
+============
+idRotation::Normalize360
+============
+*/
+void idRotation::Normalize360( void ) {
+	angle -= floor( angle / 360.0f ) * 360.0f;
+	if ( angle > 360.0f ) {
+		angle -= 360.0f;
+	}
+	else if ( angle < 0.0f ) {
+		angle += 360.0f;
+	}
+}

neo/idlib/math/Rotation.h

@@ -0,0 +1,211 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_ROTATION_H__
+#define __MATH_ROTATION_H__
+
+/*
+===============================================================================
+
+	Describes a complete rotation in degrees about an abritray axis.
+	A local rotation matrix is stored for fast rotation of multiple points.
+
+===============================================================================
+*/
+
+
+class idAngles;
+class idQuat;
+class idMat3;
+
+class idRotation {
+
+	friend class idAngles;
+	friend class idQuat;
+	friend class idMat3;
+
+public:
+						idRotation( void );
+						idRotation( const idVec3 &rotationOrigin, const idVec3 &rotationVec, const float rotationAngle );
+
+	void				Set( const idVec3 &rotationOrigin, const idVec3 &rotationVec, const float rotationAngle );
+	void				SetOrigin( const idVec3 &rotationOrigin );
+	void				SetVec( const idVec3 &rotationVec );					// has to be normalized
+	void				SetVec( const float x, const float y, const float z );	// has to be normalized
+	void				SetAngle( const float rotationAngle );
+	void				Scale( const float s );
+	void				ReCalculateMatrix( void );
+	const idVec3 &		GetOrigin( void ) const;
+	const idVec3 &		GetVec( void ) const;
+	float				GetAngle( void ) const;
+
+	idRotation			operator-() const;										// flips rotation
+	idRotation			operator*( const float s ) const;						// scale rotation
+	idRotation			operator/( const float s ) const;						// scale rotation
+	idRotation &		operator*=( const float s );							// scale rotation
+	idRotation &		operator/=( const float s );							// scale rotation
+	idVec3				operator*( const idVec3 &v ) const;						// rotate vector
+
+	friend idRotation	operator*( const float s, const idRotation &r );		// scale rotation
+	friend idVec3		operator*( const idVec3 &v, const idRotation &r );		// rotate vector
+	friend idVec3 &		operator*=( idVec3 &v, const idRotation &r );			// rotate vector
+
+	idAngles			ToAngles( void ) const;
+	idQuat				ToQuat( void ) const;
+	const idMat3 &		ToMat3( void ) const;
+	idMat4				ToMat4( void ) const;
+	idVec3				ToAngularVelocity( void ) const;
+
+	void				RotatePoint( idVec3 &point ) const;
+
+	void				Normalize180( void );
+	void				Normalize360( void );
+
+private:
+	idVec3				origin;			// origin of rotation
+	idVec3				vec;			// normalized vector to rotate around
+	float				angle;			// angle of rotation in degrees
+	mutable idMat3		axis;			// rotation axis
+	mutable bool		axisValid;		// true if rotation axis is valid
+};
+
+
+ID_INLINE idRotation::idRotation( void ) {
+}
+
+ID_INLINE idRotation::idRotation( const idVec3 &rotationOrigin, const idVec3 &rotationVec, const float rotationAngle ) {
+	origin = rotationOrigin;
+	vec = rotationVec;
+	angle = rotationAngle;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::Set( const idVec3 &rotationOrigin, const idVec3 &rotationVec, const float rotationAngle ) {
+	origin = rotationOrigin;
+	vec = rotationVec;
+	angle = rotationAngle;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::SetOrigin( const idVec3 &rotationOrigin ) {
+	origin = rotationOrigin;
+}
+
+ID_INLINE void idRotation::SetVec( const idVec3 &rotationVec ) {
+	vec = rotationVec;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::SetVec( float x, float y, float z ) {
+	vec[0] = x;
+	vec[1] = y;
+	vec[2] = z;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::SetAngle( const float rotationAngle ) {
+	angle = rotationAngle;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::Scale( const float s ) {
+	angle *= s;
+	axisValid = false;
+}
+
+ID_INLINE void idRotation::ReCalculateMatrix( void ) {
+	axisValid = false;
+	ToMat3();
+}
+
+ID_INLINE const idVec3 &idRotation::GetOrigin( void ) const {
+	return origin;
+}
+
+ID_INLINE const idVec3 &idRotation::GetVec( void ) const  {
+	return vec;
+}
+
+ID_INLINE float idRotation::GetAngle( void ) const  {
+	return angle;
+}
+
+ID_INLINE idRotation idRotation::operator-() const {
+	return idRotation( origin, vec, -angle );
+}
+
+ID_INLINE idRotation idRotation::operator*( const float s ) const {
+	return idRotation( origin, vec, angle * s );
+}
+
+ID_INLINE idRotation idRotation::operator/( const float s ) const {
+	assert( s != 0.0f );
+	return idRotation( origin, vec, angle / s );
+}
+
+ID_INLINE idRotation &idRotation::operator*=( const float s ) {
+	angle *= s;
+	axisValid = false;
+	return *this;
+}
+
+ID_INLINE idRotation &idRotation::operator/=( const float s ) {
+	assert( s != 0.0f );
+	angle /= s;
+	axisValid = false;
+	return *this;
+}
+
+ID_INLINE idVec3 idRotation::operator*( const idVec3 &v ) const {
+	if ( !axisValid ) {
+		ToMat3();
+	}
+	return ((v - origin) * axis + origin);
+}
+
+ID_INLINE idRotation operator*( const float s, const idRotation &r ) {
+	return r * s;
+}
+
+ID_INLINE idVec3 operator*( const idVec3 &v, const idRotation &r ) {
+	return r * v;
+}
+
+ID_INLINE idVec3 &operator*=( idVec3 &v, const idRotation &r ) {
+	v = r * v;
+	return v;
+}
+
+ID_INLINE void idRotation::RotatePoint( idVec3 &point ) const {
+	if ( !axisValid ) {
+		ToMat3();
+	}
+	point = ((point - origin) * axis + origin);
+}
+
+#endif /* !__MATH_ROTATION_H__ */

neo/idlib/math/Simd.h

@@ -0,0 +1,204 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_H__
+#define __MATH_SIMD_H__
+
+/*
+===============================================================================
+
+	Single Instruction Multiple Data (SIMD)
+
+	For optimal use data should be aligned on a 16 byte boundary.
+	All idSIMDProcessor routines are thread safe.
+
+===============================================================================
+*/
+
+class idSIMD {
+public:
+	static void			Init( void );
+	static void			InitProcessor( const char *module, bool forceGeneric );
+	static void			Shutdown( void );
+	static void			Test_f( const class idCmdArgs &args );
+};
+
+
+/*
+===============================================================================
+
+	virtual base class for different SIMD processors
+
+===============================================================================
+*/
+
+#ifdef _WIN32
+#define VPCALL __fastcall
+#else
+#define VPCALL
+#endif
+
+class idVec2;
+class idVec3;
+class idVec4;
+class idVec5;
+class idVec6;
+class idVecX;
+class idMat2;
+class idMat3;
+class idMat4;
+class idMat5;
+class idMat6;
+class idMatX;
+class idPlane;
+class idDrawVert;
+class idJointQuat;
+class idJointMat;
+struct dominantTri_s;
+
+const int MIXBUFFER_SAMPLES = 4096;
+
+typedef enum {
+	SPEAKER_LEFT = 0,
+	SPEAKER_RIGHT,
+	SPEAKER_CENTER,
+	SPEAKER_LFE,
+	SPEAKER_BACKLEFT,
+	SPEAKER_BACKRIGHT
+} speakerLabel;
+
+
+class idSIMDProcessor {
+public:
+									idSIMDProcessor( void ) { cpuid = CPUID_NONE; }
+
+	cpuid_t							cpuid;
+
+	virtual const char * VPCALL		GetName( void ) const = 0;
+
+	virtual void VPCALL Add( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL Add( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+	virtual void VPCALL Sub( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL Sub( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+	virtual void VPCALL Mul( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL Mul( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+	virtual void VPCALL Div( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL Div( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+	virtual void VPCALL MulAdd( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL MulAdd( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+	virtual void VPCALL MulSub( float *dst,			const float constant,	const float *src,		const int count ) = 0;
+	virtual void VPCALL MulSub( float *dst,			const float *src0,		const float *src1,		const int count ) = 0;
+
+	virtual	void VPCALL Dot( float *dst,			const idVec3 &constant,	const idVec3 *src,		const int count ) = 0;
+	virtual	void VPCALL Dot( float *dst,			const idVec3 &constant,	const idPlane *src,		const int count ) = 0;
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idDrawVert *src,	const int count ) = 0;
+	virtual	void VPCALL Dot( float *dst,			const idPlane &constant,const idVec3 *src,		const int count ) = 0;
+	virtual	void VPCALL Dot( float *dst,			const idPlane &constant,const idPlane *src,		const int count ) = 0;
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idDrawVert *src,	const int count ) = 0;
+	virtual	void VPCALL Dot( float *dst,			const idVec3 *src0,		const idVec3 *src1,		const int count ) = 0;
+	virtual void VPCALL Dot( float &dot,			const float *src1,		const float *src2,		const int count ) = 0;
+
+	virtual	void VPCALL CmpGT( byte *dst,			const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpGT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpGE( byte *dst,			const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpGE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpLT( byte *dst,			const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpLT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpLE( byte *dst,			const float *src0,		const float constant,	const int count ) = 0;
+	virtual	void VPCALL CmpLE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count ) = 0;
+
+	virtual	void VPCALL MinMax( float &min,			float &max,				const float *src,		const int count ) = 0;
+	virtual	void VPCALL MinMax( idVec2 &min,		idVec2 &max,			const idVec2 *src,		const int count ) = 0;
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idVec3 *src,		const int count ) = 0;
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int count ) = 0;
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int *indexes,		const int count ) = 0;
+
+	virtual	void VPCALL Clamp( float *dst,			const float *src,		const float min,		const float max,		const int count ) = 0;
+	virtual	void VPCALL ClampMin( float *dst,		const float *src,		const float min,		const int count ) = 0;
+	virtual	void VPCALL ClampMax( float *dst,		const float *src,		const float max,		const int count ) = 0;
+
+	virtual void VPCALL Memcpy( void *dst,			const void *src,		const int count ) = 0;
+	virtual void VPCALL Memset( void *dst,			const int val,			const int count ) = 0;
+
+	// these assume 16 byte aligned and 16 byte padded memory
+	virtual void VPCALL Zero16( float *dst,			const int count ) = 0;
+	virtual void VPCALL Negate16( float *dst,		const int count ) = 0;
+	virtual void VPCALL Copy16( float *dst,			const float *src,		const int count ) = 0;
+	virtual void VPCALL Add16( float *dst,			const float *src1,		const float *src2,		const int count ) = 0;
+	virtual void VPCALL Sub16( float *dst,			const float *src1,		const float *src2,		const int count ) = 0;
+	virtual void VPCALL Mul16( float *dst,			const float *src1,		const float constant,	const int count ) = 0;
+	virtual void VPCALL AddAssign16( float *dst,	const float *src,		const int count ) = 0;
+	virtual void VPCALL SubAssign16( float *dst,	const float *src,		const int count ) = 0;
+	virtual void VPCALL MulAssign16( float *dst,	const float constant,	const int count ) = 0;
+
+	// idMatX operations
+	virtual void VPCALL MatX_MultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_MultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_MultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_TransposeMultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_TransposeMultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_TransposeMultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec ) = 0;
+	virtual void VPCALL MatX_MultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 ) = 0;
+	virtual void VPCALL MatX_TransposeMultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 ) = 0;
+	virtual void VPCALL MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip = 0 ) = 0;
+	virtual void VPCALL MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n ) = 0;
+	virtual bool VPCALL MatX_LDLTFactor( idMatX &mat, idVecX &invDiag, const int n ) = 0;
+
+	// rendering
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) = 0;
+	virtual void VPCALL ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints ) = 0;
+	virtual void VPCALL ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints ) = 0;
+	virtual void VPCALL TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) = 0;
+	virtual void VPCALL UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) = 0;
+	virtual void VPCALL TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights ) = 0;
+	virtual void VPCALL TracePointCull( byte *cullBits, byte &totalOr, const float radius, const idPlane *planes, const idDrawVert *verts, const int numVerts ) = 0;
+	virtual void VPCALL DecalPointCull( byte *cullBits, const idPlane *planes, const idDrawVert *verts, const int numVerts ) = 0;
+	virtual void VPCALL OverlayPointCull( byte *cullBits, idVec2 *texCoords, const idPlane *planes, const idDrawVert *verts, const int numVerts ) = 0;
+	virtual void VPCALL DeriveTriPlanes( idPlane *planes, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes ) = 0;
+	virtual void VPCALL DeriveTangents( idPlane *planes, idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes ) = 0;
+	virtual void VPCALL DeriveUnsmoothedTangents( idDrawVert *verts, const dominantTri_s *dominantTris, const int numVerts ) = 0;
+	virtual void VPCALL NormalizeTangents( idDrawVert *verts, const int numVerts ) = 0;
+	virtual void VPCALL CreateTextureSpaceLightVectors( idVec3 *lightVectors, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes ) = 0;
+	virtual void VPCALL CreateSpecularTextureCoords( idVec4 *texCoords, const idVec3 &lightOrigin, const idVec3 &viewOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes ) = 0;
+	virtual int  VPCALL CreateShadowCache( idVec4 *vertexCache, int *vertRemap, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts ) = 0;
+	virtual int  VPCALL CreateVertexProgramShadowCache( idVec4 *vertexCache, const idDrawVert *verts, const int numVerts ) = 0;
+
+	// sound mixing
+	virtual void VPCALL UpSamplePCMTo44kHz( float *dest, const short *pcm, const int numSamples, const int kHz, const int numChannels ) = 0;
+	virtual void VPCALL UpSampleOGGTo44kHz( float *dest, const float * const *ogg, const int numSamples, const int kHz, const int numChannels ) = 0;
+	virtual void VPCALL MixSoundTwoSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] ) = 0;
+	virtual void VPCALL MixSoundTwoSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] ) = 0;
+	virtual void VPCALL MixSoundSixSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] ) = 0;
+	virtual void VPCALL MixSoundSixSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] ) = 0;
+	virtual void VPCALL MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples ) = 0;
+};
+
+// pointer to SIMD processor
+extern idSIMDProcessor *SIMDProcessor;
+
+#endif /* !__MATH_SIMD_H__ */

neo/idlib/math/Simd_3DNow.cpp

@@ -0,0 +1,297 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+#include "Simd_Generic.h"
+#include "Simd_MMX.h"
+#include "Simd_3DNow.h"
+
+
+//===============================================================
+//
+//	3DNow! implementation of idSIMDProcessor
+//
+//===============================================================
+
+#ifdef _WIN32
+
+/*
+============
+idSIMD_3DNow::GetName
+============
+*/
+const char * idSIMD_3DNow::GetName( void ) const {
+	return "MMX & 3DNow!";
+}
+
+// Very optimized memcpy() routine for all AMD Athlon and Duron family.
+// This code uses any of FOUR different basic copy methods, depending
+// on the transfer size.
+// NOTE:  Since this code uses MOVNTQ (also known as "Non-Temporal MOV" or
+// "Streaming Store"), and also uses the software prefetchnta instructions,
+// be sure you're running on Athlon/Duron or other recent CPU before calling!
+
+#define TINY_BLOCK_COPY 64       // upper limit for movsd type copy
+// The smallest copy uses the X86 "movsd" instruction, in an optimized
+// form which is an "unrolled loop".
+
+#define IN_CACHE_COPY 64 * 1024  // upper limit for movq/movq copy w/SW prefetch
+// Next is a copy that uses the MMX registers to copy 8 bytes at a time,
+// also using the "unrolled loop" optimization.   This code uses
+// the software prefetch instruction to get the data into the cache.
+
+#define UNCACHED_COPY 197 * 1024 // upper limit for movq/movntq w/SW prefetch
+// For larger blocks, which will spill beyond the cache, it's faster to
+// use the Streaming Store instruction MOVNTQ.   This write instruction
+// bypasses the cache and writes straight to main memory.  This code also
+// uses the software prefetch instruction to pre-read the data.
+// USE 64 * 1024 FOR THIS VALUE IF YOU'RE ALWAYS FILLING A "CLEAN CACHE"
+
+#define BLOCK_PREFETCH_COPY  infinity // no limit for movq/movntq w/block prefetch 
+#define CACHEBLOCK 80h // number of 64-byte blocks (cache lines) for block prefetch
+// For the largest size blocks, a special technique called Block Prefetch
+// can be used to accelerate the read operations.   Block Prefetch reads
+// one address per cache line, for a series of cache lines, in a short loop.
+// This is faster than using software prefetch.  The technique is great for
+// getting maximum read bandwidth, especially in DDR memory systems.
+
+/*
+================
+idSIMD_3DNow::Memcpy
+
+  optimized memory copy routine that handles all alignment cases and block sizes efficiently
+================
+*/
+void VPCALL idSIMD_3DNow::Memcpy( void *dest, const void *src, const int n ) {
+  __asm {
+
+	mov		ecx, [n]					// number of bytes to copy
+	mov		edi, [dest]					// destination
+	mov		esi, [src]					// source
+	mov		ebx, ecx					// keep a copy of count
+
+	cld
+	cmp		ecx, TINY_BLOCK_COPY
+	jb		$memcpy_ic_3				// tiny? skip mmx copy
+
+	cmp		ecx, 32*1024				// don't align between 32k-64k because
+	jbe		$memcpy_do_align			//  it appears to be slower
+	cmp		ecx, 64*1024
+	jbe		$memcpy_align_done
+$memcpy_do_align:
+	mov		ecx, 8						// a trick that's faster than rep movsb...
+	sub		ecx, edi					// align destination to qword
+	and		ecx, 111b					// get the low bits
+	sub		ebx, ecx					// update copy count
+	neg		ecx							// set up to jump into the array
+	add		ecx, offset $memcpy_align_done
+	jmp		ecx							// jump to array of movsb's
+
+align 4
+	movsb
+	movsb
+	movsb
+	movsb
+	movsb
+	movsb
+	movsb
+	movsb
+
+$memcpy_align_done:						// destination is dword aligned
+	mov		ecx, ebx					// number of bytes left to copy
+	shr		ecx, 6						// get 64-byte block count
+	jz		$memcpy_ic_2				// finish the last few bytes
+
+	cmp		ecx, IN_CACHE_COPY/64		// too big 4 cache? use uncached copy
+	jae		$memcpy_uc_test
+
+// This is small block copy that uses the MMX registers to copy 8 bytes
+// at a time.  It uses the "unrolled loop" optimization, and also uses
+// the software prefetch instruction to get the data into the cache.
+align 16
+$memcpy_ic_1:							// 64-byte block copies, in-cache copy
+
+	prefetchnta [esi + (200*64/34+192)]	// start reading ahead
+
+	movq	mm0, [esi+0]				// read 64 bits
+	movq	mm1, [esi+8]
+	movq	[edi+0], mm0				// write 64 bits
+	movq	[edi+8], mm1				//    note:  the normal movq writes the
+	movq	mm2, [esi+16]				//    data to cache; a cache line will be
+	movq	mm3, [esi+24]				//    allocated as needed, to store the data
+	movq	[edi+16], mm2
+	movq	[edi+24], mm3
+	movq	mm0, [esi+32]
+	movq	mm1, [esi+40]
+	movq	[edi+32], mm0
+	movq	[edi+40], mm1
+	movq	mm2, [esi+48]
+	movq	mm3, [esi+56]
+	movq	[edi+48], mm2
+	movq	[edi+56], mm3
+
+	add		esi, 64						// update source pointer
+	add		edi, 64						// update destination pointer
+	dec		ecx							// count down
+	jnz		$memcpy_ic_1				// last 64-byte block?
+
+$memcpy_ic_2:
+	mov		ecx, ebx					// has valid low 6 bits of the byte count
+$memcpy_ic_3:
+	shr		ecx, 2						// dword count
+	and		ecx, 1111b					// only look at the "remainder" bits
+	neg		ecx							// set up to jump into the array
+	add		ecx, offset $memcpy_last_few
+	jmp		ecx							// jump to array of movsd's
+
+$memcpy_uc_test:
+	cmp		ecx, UNCACHED_COPY/64		// big enough? use block prefetch copy
+	jae		$memcpy_bp_1
+
+$memcpy_64_test:
+	or		ecx, ecx					// tail end of block prefetch will jump here
+	jz		$memcpy_ic_2				// no more 64-byte blocks left
+
+// For larger blocks, which will spill beyond the cache, it's faster to
+// use the Streaming Store instruction MOVNTQ.   This write instruction
+// bypasses the cache and writes straight to main memory.  This code also
+// uses the software prefetch instruction to pre-read the data.
+align 16
+$memcpy_uc_1:							// 64-byte blocks, uncached copy
+
+	prefetchnta [esi + (200*64/34+192)]	// start reading ahead
+
+	movq	mm0,[esi+0]					// read 64 bits
+	add		edi,64						// update destination pointer
+	movq	mm1,[esi+8]
+	add		esi,64						// update source pointer
+	movq	mm2,[esi-48]
+	movntq	[edi-64], mm0				// write 64 bits, bypassing the cache
+	movq	mm0,[esi-40]				//    note: movntq also prevents the CPU
+	movntq	[edi-56], mm1				//    from READING the destination address
+	movq	mm1,[esi-32]				//    into the cache, only to be over-written
+	movntq	[edi-48], mm2				//    so that also helps performance
+	movq	mm2,[esi-24]
+	movntq	[edi-40], mm0
+	movq	mm0,[esi-16]
+	movntq	[edi-32], mm1
+	movq	mm1,[esi-8]
+	movntq	[edi-24], mm2
+	movntq	[edi-16], mm0
+	dec		ecx
+	movntq	[edi-8], mm1
+	jnz		$memcpy_uc_1				// last 64-byte block?
+
+	jmp		$memcpy_ic_2				// almost done
+
+// For the largest size blocks, a special technique called Block Prefetch
+// can be used to accelerate the read operations.   Block Prefetch reads
+// one address per cache line, for a series of cache lines, in a short loop.
+// This is faster than using software prefetch, in this case.
+// The technique is great for getting maximum read bandwidth,
+// especially in DDR memory systems.
+$memcpy_bp_1:							// large blocks, block prefetch copy
+
+	cmp		ecx, CACHEBLOCK				// big enough to run another prefetch loop?
+	jl		$memcpy_64_test				// no, back to regular uncached copy
+
+	mov		eax, CACHEBLOCK / 2			// block prefetch loop, unrolled 2X
+	add		esi, CACHEBLOCK * 64		// move to the top of the block
+align 16
+$memcpy_bp_2:
+	mov		edx, [esi-64]				// grab one address per cache line
+	mov		edx, [esi-128]				// grab one address per cache line
+	sub		esi, 128					// go reverse order
+	dec		eax							// count down the cache lines
+	jnz		$memcpy_bp_2				// keep grabbing more lines into cache
+
+	mov		eax, CACHEBLOCK				// now that it's in cache, do the copy
+align 16
+$memcpy_bp_3:
+	movq	mm0, [esi   ]				// read 64 bits
+	movq	mm1, [esi+ 8]
+	movq	mm2, [esi+16]
+	movq	mm3, [esi+24]
+	movq	mm4, [esi+32]
+	movq	mm5, [esi+40]
+	movq	mm6, [esi+48]
+	movq	mm7, [esi+56]
+	add		esi, 64						// update source pointer
+	movntq	[edi   ], mm0				// write 64 bits, bypassing cache
+	movntq	[edi+ 8], mm1				//    note: movntq also prevents the CPU
+	movntq	[edi+16], mm2				//    from READING the destination address 
+	movntq	[edi+24], mm3				//    into the cache, only to be over-written,
+	movntq	[edi+32], mm4				//    so that also helps performance
+	movntq	[edi+40], mm5
+	movntq	[edi+48], mm6
+	movntq	[edi+56], mm7
+	add		edi, 64						// update dest pointer
+
+	dec		eax							// count down
+
+	jnz		$memcpy_bp_3				// keep copying
+	sub		ecx, CACHEBLOCK				// update the 64-byte block count
+	jmp		$memcpy_bp_1				// keep processing chunks
+
+// The smallest copy uses the X86 "movsd" instruction, in an optimized
+// form which is an "unrolled loop".   Then it handles the last few bytes.
+align 4
+	movsd
+	movsd								// perform last 1-15 dword copies
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd								// perform last 1-7 dword copies
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd
+	movsd
+
+$memcpy_last_few:						// dword aligned from before movsd's
+	mov		ecx, ebx					// has valid low 2 bits of the byte count
+	and		ecx, 11b					// the last few cows must come home
+	jz		$memcpy_final				// no more, let's leave
+	rep		movsb						// the last 1, 2, or 3 bytes
+
+$memcpy_final: 
+	emms								// clean up the MMX state
+	sfence								// flush the write buffer
+	mov		eax, [dest]					// ret value = destination pointer
+
+    }
+}
+
+#endif /* _WIN32 */

neo/idlib/math/Simd_3DNow.h

@@ -0,0 +1,50 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_3DNOW_H__
+#define __MATH_SIMD_3DNOW_H__
+
+/*
+===============================================================================
+
+	3DNow! implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_3DNow : public idSIMD_MMX {
+#ifdef _WIN32
+public:
+	virtual const char * VPCALL GetName( void ) const;
+
+	virtual void VPCALL Memcpy( void *dst,			const void *src,		const int count );
+
+#endif
+};
+
+#endif /* !__MATH_SIMD_3DNOW_H__ */

neo/idlib/math/Simd_AltiVec.h

@@ -0,0 +1,250 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_ALTIVEC_H__
+#define __MATH_SIMD_ALTIVEC_H__
+
+/*
+===============================================================================
+
+	AltiVec implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+// Defines for enabling parts of the library
+
+// Turns on/off the simple math routines (add, sub, div, etc)
+#define ENABLE_SIMPLE_MATH	
+
+// Turns on/off the dot routines
+#define ENABLE_DOT
+
+// Turns on/off the compare routines
+#define ENABLE_COMPARES
+
+// The MinMax routines introduce a couple of bugs. In the bathroom of the alphalabs2 map, the
+// wrong surface appears in the mirror at times. It also introduces a noticable delay when map
+// data is loaded such as going through doors.
+// Turns on/off MinMax routines
+//#define ENABLE_MINMAX
+
+// Turns on/off Clamp routines
+#define ENABLE_CLAMP
+
+// Turns on/off XXX16 routines
+#define ENABLE_16ROUTINES
+
+// Turns on/off LowerTriangularSolve, LowerTriangularSolveTranspose, and MatX_LDLTFactor
+#define ENABLE_LOWER_TRIANGULAR
+
+// Turns on/off TracePointCull, DecalPointCull, and OverlayPoint
+// The Enable_Cull routines breaks the g_decals functionality, DecalPointCull is
+// the likely suspect. Bullet holes do not appear on the walls when this optimization
+// is enabled. 
+//#define ENABLE_CULL
+
+// Turns on/off DeriveTriPlanes, DeriveTangents, DeriveUnsmoothedTangents, NormalizeTangents
+#define ENABLE_DERIVE
+
+// Turns on/off CreateTextureSpaceLightVectors, CreateShadowCache, CreateVertexProgramShadowCache
+#define ENABLE_CREATE
+
+// Turns on/off the sound routines
+#define ENABLE_SOUND_ROUTINES
+
+// Turns on/off the stuff that isn't on elsewhere
+// Currently: BlendJoints, TransformJoints, UntransformJoints, ConvertJointQuatsToJointMats, and
+// ConvertJointMatsToJointQuats
+#define LIVE_VICARIOUSLY
+
+// This assumes that the dest (and mixBuffer) array to the sound functions is aligned. If this is not true, we take a large
+// performance hit from having to do unaligned stores
+//#define SOUND_DEST_ALIGNED
+
+// This assumes that the vertexCache array to CreateShadowCache and CreateVertexProgramShadowCache is aligned. If it's not,
+// then we take a big performance hit from unaligned stores.
+//#define VERTEXCACHE_ALIGNED
+
+// This turns on support for PPC intrinsics in the SIMD_AltiVec.cpp file. Right now it's only used for frsqrte. GCC 
+// supports these intrinsics but XLC does not.
+#define PPC_INTRINSICS
+
+// This assumes that the idDrawVert array that is used in DeriveUnsmoothedTangents is aligned. If its not aligned,
+// then we don't get any speedup
+//#define DERIVE_UNSMOOTH_DRAWVERT_ALIGNED
+
+// Disable DRAWVERT_PADDED since we disabled the ENABLE_CULL optimizations and the default
+// implementation does not allow for the extra padding.
+// This assumes that idDrawVert has been padded by 4 bytes so that xyz always starts at an aligned
+// address
+//#define DRAWVERT_PADDED
+
+class idSIMD_AltiVec : public idSIMD_Generic {
+#if defined(MACOS_X) && defined(__ppc__)
+public:
+
+	virtual const char * VPCALL GetName( void ) const;
+
+#ifdef ENABLE_SIMPLE_MATH
+	// Basic math, works for both aligned and unaligned data
+	virtual void VPCALL Add( float *dst, const float constant, const float *src, const int count );
+    virtual void VPCALL Add( float *dst, const float *src0, const float *src1, const int count );
+	virtual void VPCALL Sub( float *dst, const float constant, const float *src, const int count );
+	virtual void VPCALL Sub( float *dst, const float *src0, const float *src1, const int count );
+ 	virtual void VPCALL Mul( float *dst, const float constant, const float *src, const int count);
+	virtual void VPCALL Mul( float *dst, const float *src0, const float *src1, const int count );
+	virtual void VPCALL Div( float *dst, const float constant, const float *divisor, const int count );
+	virtual void VPCALL Div( float *dst, const float *src0, const float *src1, const int count ); 
+	virtual void VPCALL MulAdd( float *dst, const float constant, const float *src, const int count );
+	virtual void VPCALL MulAdd( float *dst, const float *src0, const float *src1, const int count );
+	virtual void VPCALL MulSub( float *dst, const float constant, const float *src, const int count );
+	virtual void VPCALL MulSub( float *dst, const float *src0, const float *src1, const int count ); 
+#endif
+
+#ifdef ENABLE_DOT
+	// Dot products, expects data structures in contiguous memory
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 *src0,		const idVec3 *src1,		const int count );
+	virtual void VPCALL Dot( float &dot,			const float *src1,		const float *src2,		const int count );
+#endif
+
+#ifdef ENABLE_COMPARES
+	// Comparisons, works for both aligned and unaligned data
+	virtual void VPCALL CmpGT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+#endif
+
+#ifdef ENABLE_MINMAX	
+	// Min/Max. Expects data structures in contiguous memory
+	virtual void VPCALL MinMax( float &min,			float &max,				const float *src,		const int count );
+	virtual	void VPCALL MinMax( idVec2 &min,		idVec2 &max,			const idVec2 *src,		const int count );
+	virtual void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idVec3 *src,		const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int *indexes,		const int count );
+#endif
+
+#ifdef ENABLE_CLAMP
+	// Clamp operations. Works for both aligned and unaligned data
+	virtual void VPCALL Clamp( float *dst,			const float *src,		const float min,		const float max,		const int count );
+	virtual void VPCALL ClampMin( float *dst,		const float *src,		const float min,		const int count );
+	virtual void VPCALL ClampMax( float *dst,		const float *src,		const float max,		const int count );
+#endif
+
+    // These are already using memcpy and memset functions. Leaving default implementation
+//	virtual void VPCALL Memcpy( void *dst,			const void *src,		const int count );
+//	virtual void VPCALL Memset( void *dst,			const int val,			const int count );
+
+#ifdef ENABLE_16ROUTINES
+	// Operations that expect 16-byte aligned data and 16-byte padded memory (with zeros), generally faster
+	virtual void VPCALL Zero16( float *dst,			const int count );
+	virtual void VPCALL Negate16( float *dst,		const int count );
+	virtual void VPCALL Copy16( float *dst,			const float *src,		const int count );
+	virtual void VPCALL Add16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Sub16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Mul16( float *dst,			const float *src1,		const float constant,	const int count );
+	virtual void VPCALL AddAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL SubAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL MulAssign16( float *dst,	const float constant,	const int count );
+#endif
+
+//  Most of these deal with tiny matrices or vectors, generally not worth altivec'ing since
+//  the scalar code is already really fast
+
+//	virtual void VPCALL MatX_MultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_MultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_MultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_TransposeMultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_TransposeMultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_TransposeMultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+//	virtual void VPCALL MatX_MultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+//	virtual void VPCALL MatX_TransposeMultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+
+#ifdef ENABLE_LOWER_TRIANGULAR
+	virtual void VPCALL MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip = 0 );
+	virtual void VPCALL MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n );
+	virtual bool VPCALL MatX_LDLTFactor( idMatX &mat, idVecX &invDiag, const int n );
+#endif
+#ifdef LIVE_VICARIOUSLY
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints );
+	virtual void VPCALL ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints );
+	virtual void VPCALL ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints );
+#endif
+
+#ifdef LIVE_VICARIOUSLY
+	virtual void VPCALL TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights );
+#endif
+
+#ifdef ENABLE_CULL
+	virtual void VPCALL TracePointCull( byte *cullBits, byte &totalOr, const float radius, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL DecalPointCull( byte *cullBits, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL OverlayPointCull( byte *cullBits, idVec2 *texCoords, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+#endif
+
+#ifdef ENABLE_DERIVE
+	virtual void VPCALL DeriveTriPlanes( idPlane *planes, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveTangents( idPlane *planes, idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveUnsmoothedTangents( idDrawVert *verts, const dominantTri_s *dominantTris, const int numVerts );
+	virtual void VPCALL NormalizeTangents( idDrawVert *verts, const int numVerts );
+#endif	
+
+#ifdef ENABLE_CREATE
+	virtual void VPCALL CreateTextureSpaceLightVectors( idVec3 *lightVectors, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL CreateSpecularTextureCoords( idVec4 *texCoords, const idVec3 &lightOrigin, const idVec3 &viewOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual int  VPCALL CreateShadowCache( idVec4 *vertexCache, int *vertRemap, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts );
+	virtual int  VPCALL CreateVertexProgramShadowCache( idVec4 *vertexCache, const idDrawVert *verts, const int numVerts );
+#endif
+
+#ifdef ENABLE_SOUND_ROUTINES
+	// Sound upsampling and mixing routines, works for aligned and unaligned data
+	virtual void VPCALL UpSamplePCMTo44kHz( float *dest, const short *pcm, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL UpSampleOGGTo44kHz( float *dest, const float * const *ogg, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL MixSoundTwoSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundTwoSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundSixSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixSoundSixSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples );
+#endif
+#endif
+
+};
+
+#endif /* !__MATH_SIMD_ALTIVEC_H__ */

neo/idlib/math/Simd_Generic.h

@@ -0,0 +1,137 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_GENERIC_H__
+#define __MATH_SIMD_GENERIC_H__
+
+/*
+===============================================================================
+
+	Generic implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_Generic : public idSIMDProcessor {
+public:
+	virtual const char * VPCALL GetName( void ) const;
+
+	virtual void VPCALL Add( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Add( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Sub( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Sub( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Mul( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Mul( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Div( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Div( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL MulAdd( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL MulAdd( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL MulSub( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL MulSub( float *dst,			const float *src0,		const float *src1,		const int count );
+
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 *src0,		const idVec3 *src1,		const int count );
+	virtual void VPCALL Dot( float &dot,			const float *src1,		const float *src2,		const int count );
+
+	virtual void VPCALL CmpGT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+
+	virtual void VPCALL MinMax( float &min,			float &max,				const float *src,		const int count );
+	virtual	void VPCALL MinMax( idVec2 &min,		idVec2 &max,			const idVec2 *src,		const int count );
+	virtual void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idVec3 *src,		const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int *indexes,		const int count );
+
+	virtual void VPCALL Clamp( float *dst,			const float *src,		const float min,		const float max,		const int count );
+	virtual void VPCALL ClampMin( float *dst,		const float *src,		const float min,		const int count );
+	virtual void VPCALL ClampMax( float *dst,		const float *src,		const float max,		const int count );
+
+	virtual void VPCALL Memcpy( void *dst,			const void *src,		const int count );
+	virtual void VPCALL Memset( void *dst,			const int val,			const int count );
+
+	virtual void VPCALL Zero16( float *dst,			const int count );
+	virtual void VPCALL Negate16( float *dst,		const int count );
+	virtual void VPCALL Copy16( float *dst,			const float *src,		const int count );
+	virtual void VPCALL Add16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Sub16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Mul16( float *dst,			const float *src1,		const float constant,	const int count );
+	virtual void VPCALL AddAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL SubAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL MulAssign16( float *dst,	const float constant,	const int count );
+
+	virtual void VPCALL MatX_MultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+	virtual void VPCALL MatX_TransposeMultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+	virtual void VPCALL MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip = 0 );
+	virtual void VPCALL MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n );
+	virtual bool VPCALL MatX_LDLTFactor( idMatX &mat, idVecX &invDiag, const int n );
+
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints );
+	virtual void VPCALL ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints );
+	virtual void VPCALL ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints );
+	virtual void VPCALL TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights );
+	virtual void VPCALL TracePointCull( byte *cullBits, byte &totalOr, const float radius, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL DecalPointCull( byte *cullBits, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL OverlayPointCull( byte *cullBits, idVec2 *texCoords, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL DeriveTriPlanes( idPlane *planes, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveTangents( idPlane *planes, idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveUnsmoothedTangents( idDrawVert *verts, const dominantTri_s *dominantTris, const int numVerts );
+	virtual void VPCALL NormalizeTangents( idDrawVert *verts, const int numVerts );
+	virtual void VPCALL CreateTextureSpaceLightVectors( idVec3 *lightVectors, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL CreateSpecularTextureCoords( idVec4 *texCoords, const idVec3 &lightOrigin, const idVec3 &viewOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual int  VPCALL CreateShadowCache( idVec4 *vertexCache, int *vertRemap, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts );
+	virtual int  VPCALL CreateVertexProgramShadowCache( idVec4 *vertexCache, const idDrawVert *verts, const int numVerts );
+
+	virtual void VPCALL UpSamplePCMTo44kHz( float *dest, const short *pcm, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL UpSampleOGGTo44kHz( float *dest, const float * const *ogg, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL MixSoundTwoSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundTwoSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundSixSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixSoundSixSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples );
+};
+
+#endif /* !__MATH_SIMD_GENERIC_H__ */

neo/idlib/math/Simd_MMX.cpp

@@ -0,0 +1,367 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+#include "Simd_Generic.h"
+#include "Simd_MMX.h"
+
+
+//===============================================================
+//
+//	MMX implementation of idSIMDProcessor
+//
+//===============================================================
+
+#if defined(MACOS_X) && defined(__i386__)
+/*
+============
+idSIMD_MMX::GetName
+============
+*/
+const char * idSIMD_MMX::GetName( void ) const {
+	return "MMX";
+}
+
+#elif defined(_WIN32)
+
+#define EMMS_INSTRUCTION		__asm emms
+
+/*
+============
+idSIMD_MMX::GetName
+============
+*/
+const char * idSIMD_MMX::GetName( void ) const {
+	return "MMX";
+}
+
+/*
+================
+MMX_Memcpy8B
+================
+*/
+void MMX_Memcpy8B( void *dest, const void *src, const int count ) {
+	_asm { 
+        mov		esi, src 
+        mov		edi, dest 
+        mov		ecx, count 
+        shr		ecx, 3			// 8 bytes per iteration 
+
+loop1: 
+        movq	mm1,  0[ESI]	// Read in source data 
+        movntq	0[EDI], mm1		// Non-temporal stores 
+
+        add		esi, 8
+        add		edi, 8
+        dec		ecx 
+        jnz		loop1 
+
+	} 
+	EMMS_INSTRUCTION
+}
+
+/*
+================
+MMX_Memcpy64B
+
+  165MB/sec
+================
+*/
+void MMX_Memcpy64B( void *dest, const void *src, const int count ) {
+	_asm { 
+        mov		esi, src 
+        mov		edi, dest 
+        mov		ecx, count 
+        shr		ecx, 6		// 64 bytes per iteration
+
+loop1: 
+        prefetchnta 64[ESI]	// Prefetch next loop, non-temporal 
+        prefetchnta 96[ESI] 
+
+        movq mm1,  0[ESI]	// Read in source data 
+        movq mm2,  8[ESI] 
+        movq mm3, 16[ESI] 
+        movq mm4, 24[ESI] 
+        movq mm5, 32[ESI] 
+        movq mm6, 40[ESI] 
+        movq mm7, 48[ESI] 
+        movq mm0, 56[ESI] 
+
+        movntq  0[EDI], mm1	// Non-temporal stores 
+        movntq  8[EDI], mm2 
+        movntq 16[EDI], mm3 
+        movntq 24[EDI], mm4 
+        movntq 32[EDI], mm5 
+        movntq 40[EDI], mm6 
+        movntq 48[EDI], mm7 
+        movntq 56[EDI], mm0 
+
+        add		esi, 64 
+        add		edi, 64 
+        dec		ecx 
+        jnz		loop1 
+	} 
+	EMMS_INSTRUCTION
+}
+
+/*
+================
+MMX_Memcpy2kB
+
+  240MB/sec
+================
+*/
+void MMX_Memcpy2kB( void *dest, const void *src, const int count ) {
+	byte *tbuf = (byte *)_alloca16(2048);
+	__asm { 
+		push	ebx
+        mov		esi, src
+        mov		ebx, count
+        shr		ebx, 11		// 2048 bytes at a time 
+        mov		edi, dest
+
+loop2k:
+        push	edi			// copy 2k into temporary buffer
+        mov		edi, tbuf
+        mov		ecx, 32
+
+loopMemToL1: 
+        prefetchnta 64[ESI] // Prefetch next loop, non-temporal
+        prefetchnta 96[ESI]
+
+        movq mm1,  0[ESI]	// Read in source data
+        movq mm2,  8[ESI]
+        movq mm3, 16[ESI]
+        movq mm4, 24[ESI]
+        movq mm5, 32[ESI]
+        movq mm6, 40[ESI]
+        movq mm7, 48[ESI]
+        movq mm0, 56[ESI]
+
+        movq  0[EDI], mm1	// Store into L1
+        movq  8[EDI], mm2
+        movq 16[EDI], mm3
+        movq 24[EDI], mm4
+        movq 32[EDI], mm5
+        movq 40[EDI], mm6
+        movq 48[EDI], mm7
+        movq 56[EDI], mm0
+        add		esi, 64
+        add		edi, 64
+        dec		ecx
+        jnz		loopMemToL1
+
+        pop		edi			// Now copy from L1 to system memory
+        push	esi
+        mov		esi, tbuf
+        mov		ecx, 32
+
+loopL1ToMem:
+        movq mm1, 0[ESI]	// Read in source data from L1
+        movq mm2, 8[ESI]
+        movq mm3, 16[ESI]
+        movq mm4, 24[ESI]
+        movq mm5, 32[ESI]
+        movq mm6, 40[ESI]
+        movq mm7, 48[ESI]
+        movq mm0, 56[ESI]
+
+        movntq 0[EDI], mm1	// Non-temporal stores
+        movntq 8[EDI], mm2
+        movntq 16[EDI], mm3
+        movntq 24[EDI], mm4
+        movntq 32[EDI], mm5
+        movntq 40[EDI], mm6
+        movntq 48[EDI], mm7
+        movntq 56[EDI], mm0
+
+        add		esi, 64
+        add		edi, 64
+        dec		ecx
+        jnz		loopL1ToMem
+
+        pop		esi			// Do next 2k block
+        dec		ebx
+        jnz		loop2k
+		pop		ebx
+	}
+	EMMS_INSTRUCTION
+}
+
+
+/*
+================
+idSIMD_MMX::Memcpy
+
+  optimized memory copy routine that handles all alignment cases and block sizes efficiently
+================
+*/
+void VPCALL idSIMD_MMX::Memcpy( void *dest0, const void *src0, const int count0 ) {
+	// if copying more than 16 bytes and we can copy 8 byte aligned
+	if ( count0 > 16 && !( ( (int)dest0 ^ (int)src0 ) & 7 ) ) {
+		byte *dest = (byte *)dest0;
+		byte *src = (byte *)src0;
+
+		// copy up to the first 8 byte aligned boundary
+		int count = ((int)dest) & 7;
+		memcpy( dest, src, count );
+		dest += count;
+		src += count;
+		count = count0 - count;
+
+		// if there are multiple blocks of 2kB
+		if ( count & ~4095 ) {
+			MMX_Memcpy2kB( dest, src, count );
+			src += (count & ~2047);
+			dest += (count & ~2047);
+			count &= 2047;
+		}
+
+		// if there are blocks of 64 bytes
+		if ( count & ~63 ) {
+			MMX_Memcpy64B( dest, src, count );
+			src += (count & ~63);
+			dest += (count & ~63);
+			count &= 63;
+		}
+
+		// if there are blocks of 8 bytes
+		if ( count & ~7 ) {
+			MMX_Memcpy8B( dest, src, count );
+			src += (count & ~7);
+			dest += (count & ~7);
+			count &= 7;
+		}
+
+		// copy any remaining bytes
+		memcpy( dest, src, count );
+	} else {
+		// use the regular one if we cannot copy 8 byte aligned
+		memcpy( dest0, src0, count0 );
+	}
+
+	// the MMX_Memcpy* functions use MOVNTQ, issue a fence operation
+	__asm {
+		sfence
+	}
+}
+
+/*
+================
+idSIMD_MMX::Memset
+================
+*/
+void VPCALL idSIMD_MMX::Memset( void* dest0, const int val, const int count0 ) {
+	union {
+		byte	bytes[8];
+		word	words[4];
+		dword	dwords[2];
+	} dat;
+
+	byte *dest = (byte *)dest0;
+	int count = count0;
+
+	while ( count > 0 && (((int)dest) & 7) ) {
+		*dest = val;
+		dest++;
+		count--;
+	}
+	if ( !count ) {
+		return;
+	}
+
+	dat.bytes[0] = val;
+	dat.bytes[1] = val;
+	dat.words[1] = dat.words[0];
+	dat.dwords[1] = dat.dwords[0];
+
+	if ( count >= 64 ) {
+		__asm {
+			mov edi, dest 
+			mov ecx, count 
+			shr ecx, 6				// 64 bytes per iteration 
+			movq mm1, dat			// Read in source data 
+			movq mm2, mm1
+			movq mm3, mm1
+			movq mm4, mm1
+			movq mm5, mm1
+			movq mm6, mm1
+			movq mm7, mm1
+			movq mm0, mm1
+loop1: 
+			movntq  0[EDI], mm1		// Non-temporal stores 
+			movntq  8[EDI], mm2 
+			movntq 16[EDI], mm3 
+			movntq 24[EDI], mm4 
+			movntq 32[EDI], mm5 
+			movntq 40[EDI], mm6 
+			movntq 48[EDI], mm7 
+			movntq 56[EDI], mm0 
+
+			add edi, 64 
+			dec ecx 
+			jnz loop1 
+		}
+		dest += ( count & ~63 );
+		count &= 63;
+	}
+
+	if ( count >= 8 ) {
+		__asm {
+			mov edi, dest 
+			mov ecx, count 
+			shr ecx, 3				// 8 bytes per iteration 
+			movq mm1, dat			// Read in source data 
+loop2: 
+			movntq  0[EDI], mm1		// Non-temporal stores 
+
+			add edi, 8
+			dec ecx 
+			jnz loop2
+		}
+		dest += (count & ~7);
+		count &= 7;
+	}
+
+	while ( count > 0 ) {
+		*dest = val;
+		dest++;
+		count--;
+	}
+
+	EMMS_INSTRUCTION 
+
+	// the MMX_Memcpy* functions use MOVNTQ, issue a fence operation
+	__asm {
+		sfence
+	}
+}
+
+#endif /* _WIN32 */

neo/idlib/math/Simd_MMX.h

@@ -0,0 +1,54 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_MMX_H__
+#define __MATH_SIMD_MMX_H__
+
+/*
+===============================================================================
+
+	MMX implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_MMX : public idSIMD_Generic {
+public:
+#if defined(MACOS_X) && defined(__i386__)
+	virtual const char * VPCALL GetName( void ) const;
+
+#elif defined(_WIN32)
+	virtual const char * VPCALL GetName( void ) const;
+
+	virtual void VPCALL Memcpy( void *dst,			const void *src,		const int count );
+	virtual void VPCALL Memset( void *dst,			const int val,			const int count );
+
+#endif
+};
+
+#endif /* !__MATH_SIMD_MMX_H__ */

neo/idlib/math/Simd_SSE.h

@@ -0,0 +1,143 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_SSE_H__
+#define __MATH_SIMD_SSE_H__
+
+/*
+===============================================================================
+
+	SSE implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_SSE : public idSIMD_MMX {
+public:
+#if defined(MACOS_X) && defined(__i386__)
+	virtual const char * VPCALL GetName( void ) const;
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idDrawVert *src,	const int count );	
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int *indexes,		const int count );	
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idPlane *src,		const int count );	
+
+#elif defined(_WIN32)
+	virtual const char * VPCALL GetName( void ) const;
+
+	virtual void VPCALL Add( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Add( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Sub( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Sub( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Mul( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Mul( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL Div( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL Div( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL MulAdd( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL MulAdd( float *dst,			const float *src0,		const float *src1,		const int count );
+	virtual void VPCALL MulSub( float *dst,			const float constant,	const float *src,		const int count );
+	virtual void VPCALL MulSub( float *dst,			const float *src0,		const float *src1,		const int count );
+
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 &constant,	const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idVec3 *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idPlane *src,		const int count );
+	virtual void VPCALL Dot( float *dst,			const idPlane &constant,const idDrawVert *src,	const int count );
+	virtual void VPCALL Dot( float *dst,			const idVec3 *src0,		const idVec3 *src1,		const int count );
+	virtual void VPCALL Dot( float &dot,			const float *src1,		const float *src2,		const int count );
+
+	virtual void VPCALL CmpGT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpGE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const float *src0,		const float constant,	const int count );
+	virtual void VPCALL CmpLE( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );
+
+	virtual void VPCALL MinMax( float &min,			float &max,				const float *src,		const int count );
+	virtual	void VPCALL MinMax( idVec2 &min,		idVec2 &max,			const idVec2 *src,		const int count );
+	virtual void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idVec3 *src,		const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int count );
+	virtual	void VPCALL MinMax( idVec3 &min,		idVec3 &max,			const idDrawVert *src,	const int *indexes,		const int count );
+
+	virtual void VPCALL Clamp( float *dst,			const float *src,		const float min,		const float max,		const int count );
+	virtual void VPCALL ClampMin( float *dst,		const float *src,		const float min,		const int count );
+	virtual void VPCALL ClampMax( float *dst,		const float *src,		const float max,		const int count );
+
+	virtual void VPCALL Zero16( float *dst,			const int count );
+	virtual void VPCALL Negate16( float *dst,		const int count );
+	virtual void VPCALL Copy16( float *dst,			const float *src,		const int count );
+	virtual void VPCALL Add16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Sub16( float *dst,			const float *src1,		const float *src2,		const int count );
+	virtual void VPCALL Mul16( float *dst,			const float *src1,		const float constant,	const int count );
+	virtual void VPCALL AddAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL SubAssign16( float *dst,	const float *src,		const int count );
+	virtual void VPCALL MulAssign16( float *dst,	const float constant,	const int count );
+
+	virtual void VPCALL MatX_MultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplyVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplyAddVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_TransposeMultiplySubVecX( idVecX &dst, const idMatX &mat, const idVecX &vec );
+	virtual void VPCALL MatX_MultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+	virtual void VPCALL MatX_TransposeMultiplyMatX( idMatX &dst, const idMatX &m1, const idMatX &m2 );
+	virtual void VPCALL MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip = 0 );
+	virtual void VPCALL MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n );
+	virtual bool VPCALL MatX_LDLTFactor( idMatX &mat, idVecX &invDiag, const int n );
+
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints );
+	virtual void VPCALL ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints );
+	virtual void VPCALL ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints );
+	virtual void VPCALL TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint );
+	virtual void VPCALL TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights );
+	virtual void VPCALL TracePointCull( byte *cullBits, byte &totalOr, const float radius, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL DecalPointCull( byte *cullBits, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL OverlayPointCull( byte *cullBits, idVec2 *texCoords, const idPlane *planes, const idDrawVert *verts, const int numVerts );
+	virtual void VPCALL DeriveTriPlanes( idPlane *planes, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveTangents( idPlane *planes, idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL DeriveUnsmoothedTangents( idDrawVert *verts, const dominantTri_s *dominantTris, const int numVerts );
+	virtual void VPCALL NormalizeTangents( idDrawVert *verts, const int numVerts );
+	virtual void VPCALL CreateTextureSpaceLightVectors( idVec3 *lightVectors, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual void VPCALL CreateSpecularTextureCoords( idVec4 *texCoords, const idVec3 &lightOrigin, const idVec3 &viewOrigin, const idDrawVert *verts, const int numVerts, const int *indexes, const int numIndexes );
+	virtual int  VPCALL CreateShadowCache( idVec4 *vertexCache, int *vertRemap, const idVec3 &lightOrigin, const idDrawVert *verts, const int numVerts );
+	virtual int  VPCALL CreateVertexProgramShadowCache( idVec4 *vertexCache, const idDrawVert *verts, const int numVerts );
+
+	virtual void VPCALL UpSamplePCMTo44kHz( float *dest, const short *pcm, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL UpSampleOGGTo44kHz( float *dest, const float * const *ogg, const int numSamples, const int kHz, const int numChannels );
+	virtual void VPCALL MixSoundTwoSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundTwoSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[2], const float currentV[2] );
+	virtual void VPCALL MixSoundSixSpeakerMono( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixSoundSixSpeakerStereo( float *mixBuffer, const float *samples, const int numSamples, const float lastV[6], const float currentV[6] );
+	virtual void VPCALL MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples );
+
+#endif
+};
+
+#endif /* !__MATH_SIMD_SSE_H__ */

neo/idlib/math/Simd_SSE2.cpp

@@ -0,0 +1,877 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+#include "Simd_Generic.h"
+#include "Simd_MMX.h"
+#include "Simd_SSE.h"
+#include "Simd_SSE2.h"
+
+
+//===============================================================
+//
+//	SSE2 implementation of idSIMDProcessor
+//
+//===============================================================
+#if defined(MACOS_X) && defined(__i386__)
+
+#include <xmmintrin.h>
+
+#define SHUFFLEPS( x, y, z, w )		(( (x) & 3 ) << 6 | ( (y) & 3 ) << 4 | ( (z) & 3 ) << 2 | ( (w) & 3 ))
+#define R_SHUFFLEPS( x, y, z, w )	(( (w) & 3 ) << 6 | ( (z) & 3 ) << 4 | ( (y) & 3 ) << 2 | ( (x) & 3 ))
+
+/*
+============
+idSIMD_SSE2::GetName
+============
+*/
+const char * idSIMD_SSE2::GetName( void ) const {
+	return "MMX & SSE & SSE2";
+}
+
+/*
+============
+idSIMD_SSE::CmpLT
+
+  dst[i] |= ( src0[i] < constant ) << bitNum;
+============
+*/
+void VPCALL idSIMD_SSE2::CmpLT( byte *dst, const byte bitNum, const float *src0, const float constant, const int count ) {
+	int i, cnt, pre, post;
+	float *aligned;
+	__m128 xmm0, xmm1;
+	__m128i xmm0i;
+	int cnt_l;
+	char *src0_p;
+	char *constant_p;
+	char *dst_p;
+	int mask_l;
+	int dst_l;
+	
+	/* if the float array is not aligned on a 4 byte boundary */
+	if ( ((int) src0) & 3 ) {
+		/* unaligned memory access */
+		pre = 0;
+		cnt = count >> 2;
+		post = count - (cnt<<2);
+
+	/*
+		__asm	mov			edx, cnt													
+		__asm	test		edx, edx													
+		__asm	je			doneCmp														
+	*/
+		cnt_l = cnt;
+		if(cnt_l != 0) {
+	/*
+		__asm	push		ebx															
+		__asm	neg			edx															
+		__asm	mov			esi, src0													
+		__asm	prefetchnta	[esi+64]													
+		__asm	movss		xmm1, constant												
+		__asm	shufps		xmm1, xmm1, R_SHUFFLEPS( 0, 0, 0, 0 )						
+		__asm	mov			edi, dst													
+		__asm	mov			cl, bitNum	
+	*/
+			cnt_l = -cnt_l;
+			src0_p = (char *) src0;
+			_mm_prefetch(src0_p+64, _MM_HINT_NTA);
+			constant_p = (char *) &constant;
+			xmm1 = _mm_load_ss((float *)constant_p);
+			xmm1 = _mm_shuffle_ps(xmm1, xmm1, R_SHUFFLEPS( 0, 0, 0, 0 ));
+			dst_p = (char *)dst;
+	/*
+			__asm loopNA:																	
+	*/
+			do {
+	/*
+		__asm	movups		xmm0, [esi]													
+		__asm	prefetchnta	[esi+128]													
+		__asm	cmpltps		xmm0, xmm1													
+		__asm	movmskps	eax, xmm0																												\
+		__asm	mov			ah, al														
+		__asm	shr			ah, 1														
+		__asm	mov			bx, ax														
+		__asm	shl			ebx, 14														
+		__asm	mov			bx, ax														
+		__asm	and			ebx, 0x01010101												
+		__asm	shl			ebx, cl														
+		__asm	or			ebx, dword ptr [edi]										
+		__asm	mov			dword ptr [edi], ebx										
+		__asm	add			esi, 16														
+		__asm	add			edi, 4														
+		__asm	inc			edx															
+		__asm	jl			loopNA														
+		__asm	pop			ebx		
+	*/
+				xmm0 = _mm_loadu_ps((float *) src0_p);
+				_mm_prefetch(src0_p+128, _MM_HINT_NTA);
+				xmm0 = _mm_cmplt_ps(xmm0, xmm1);
+				// Simplify using SSE2
+				xmm0i = (__m128i) xmm0;
+				xmm0i = _mm_packs_epi32(xmm0i, xmm0i);
+				xmm0i = _mm_packs_epi16(xmm0i, xmm0i);
+				mask_l = _mm_cvtsi128_si32(xmm0i);
+				// End 
+				mask_l = mask_l &  0x01010101;
+				mask_l = mask_l << bitNum;
+				dst_l  = *((int *) dst_p);
+				mask_l = mask_l | dst_l;
+				*((int *) dst_p) = mask_l;
+				src0_p = src0_p + 16;
+				dst_p = dst_p + 4;
+				cnt_l = cnt_l + 1;
+			} while (cnt_l < 0);
+		}													
+	}																					
+	else {																				
+		/* aligned memory access */														
+		aligned = (float *) ((((int) src0) + 15) & ~15);								
+		if ( (int)aligned > ((int)src0) + count ) {										
+			pre = count;																
+			post = 0;																	
+		}																				
+		else {																			
+			pre = aligned - src0;														
+			cnt = (count - pre) >> 2;													
+			post = count - pre - (cnt<<2);
+	/*
+			__asm	mov			edx, cnt												
+			__asm	test		edx, edx												
+			__asm	je			doneCmp													
+	*/
+			cnt_l = cnt;
+			if(cnt_l != 0) {
+	/*
+			__asm	push		ebx														
+			__asm	neg			edx														
+			__asm	mov			esi, aligned											
+			__asm	prefetchnta	[esi+64]												
+			__asm	movss		xmm1, constant											
+			__asm	shufps		xmm1, xmm1, R_SHUFFLEPS( 0, 0, 0, 0 )					
+			__asm	mov			edi, dst												
+			__asm	add			edi, pre												
+			__asm	mov			cl, bitNum
+	*/
+				cnt_l = -cnt_l;
+				src0_p = (char *) src0;
+				_mm_prefetch(src0_p+64, _MM_HINT_NTA);
+				constant_p = (char *) &constant;
+				xmm1 = _mm_load_ss((float *)constant_p);
+				xmm1 = _mm_shuffle_ps(xmm1, xmm1, R_SHUFFLEPS( 0, 0, 0, 0 ));
+				dst_p = (char *)dst;
+				dst_p = dst_p + pre;
+	/*
+			__asm loopA:																
+	*/
+			do {
+	/*
+			__asm	movaps		xmm0, [esi]												
+			__asm	prefetchnta	[esi+128]												
+			__asm	cmpltps		xmm0, xmm1												
+			__asm	movmskps	eax, xmm0																											\
+			__asm	mov			ah, al													
+			__asm	shr			ah, 1													
+			__asm	mov			bx, ax													
+			__asm	shl			ebx, 14													
+			__asm	mov			bx, ax													
+			__asm	and			ebx, 0x01010101											
+			__asm	shl			ebx, cl													
+			__asm	or			ebx, dword ptr [edi]									
+			__asm	mov			dword ptr [edi], ebx									
+			__asm	add			esi, 16													
+			__asm	add			edi, 4													
+			__asm	inc			edx														
+			__asm	jl			loopA													
+			__asm	pop			ebx
+	*/
+					xmm0 = _mm_load_ps((float *) src0_p);
+					_mm_prefetch(src0_p+128, _MM_HINT_NTA);
+					xmm0 = _mm_cmplt_ps(xmm0, xmm1);
+					// Simplify using SSE2
+					xmm0i = (__m128i) xmm0;
+					xmm0i = _mm_packs_epi32(xmm0i, xmm0i);
+					xmm0i = _mm_packs_epi16(xmm0i, xmm0i);
+					mask_l = _mm_cvtsi128_si32(xmm0i);
+					// End 
+					mask_l = mask_l &  0x01010101;
+					mask_l = mask_l << bitNum;
+					dst_l  = *((int *) dst_p);
+					mask_l = mask_l | dst_l;
+					*((int *) dst_p) = mask_l;
+					src0_p = src0_p + 16;
+					dst_p = dst_p + 4;
+					cnt_l = cnt_l + 1;
+				} while (cnt_l < 0);
+			}	
+		}
+	}	
+	/*
+	doneCmp:																			
+	*/
+	float c = constant;																	
+	for ( i = 0; i < pre; i++ ) {														
+		dst[i] |= ( src0[i] < c ) << bitNum;											
+	}																					
+ 	for ( i = count - post; i < count; i++ ) {											
+		dst[i] |= ( src0[i] < c ) << bitNum;											
+	}
+}
+
+#elif defined(_WIN32)
+
+#include <xmmintrin.h>
+
+#define SHUFFLEPS( x, y, z, w )		(( (x) & 3 ) << 6 | ( (y) & 3 ) << 4 | ( (z) & 3 ) << 2 | ( (w) & 3 ))
+#define R_SHUFFLEPS( x, y, z, w )	(( (w) & 3 ) << 6 | ( (z) & 3 ) << 4 | ( (y) & 3 ) << 2 | ( (x) & 3 ))
+#define SHUFFLEPD( x, y )			(( (x) & 1 ) << 1 | ( (y) & 1 ))
+#define R_SHUFFLEPD( x, y )			(( (y) & 1 ) << 1 | ( (x) & 1 ))
+
+
+#define ALIGN4_INIT1( X, INIT )				ALIGN16( static X[4] ) = { INIT, INIT, INIT, INIT }
+#define ALIGN4_INIT4( X, I0, I1, I2, I3 )	ALIGN16( static X[4] ) = { I0, I1, I2, I3 }
+#define ALIGN8_INIT1( X, INIT )				ALIGN16( static X[8] ) = { INIT, INIT, INIT, INIT, INIT, INIT, INIT, INIT }
+
+ALIGN8_INIT1( unsigned short SIMD_W_zero, 0 );
+ALIGN8_INIT1( unsigned short SIMD_W_maxShort, 1<<15 );
+
+ALIGN4_INIT4( unsigned long SIMD_SP_singleSignBitMask, (unsigned long) ( 1 << 31 ), 0, 0, 0 );
+ALIGN4_INIT1( unsigned long SIMD_SP_signBitMask, (unsigned long) ( 1 << 31 ) );
+ALIGN4_INIT1( unsigned long SIMD_SP_absMask, (unsigned long) ~( 1 << 31 ) );
+ALIGN4_INIT1( unsigned long SIMD_SP_infinityMask, (unsigned long) ~( 1 << 23 ) );
+
+ALIGN4_INIT1( float SIMD_SP_zero, 0.0f );
+ALIGN4_INIT1( float SIMD_SP_one, 1.0f );
+ALIGN4_INIT1( float SIMD_SP_two, 2.0f );
+ALIGN4_INIT1( float SIMD_SP_three, 3.0f );
+ALIGN4_INIT1( float SIMD_SP_four, 4.0f );
+ALIGN4_INIT1( float SIMD_SP_maxShort, (1<<15) );
+ALIGN4_INIT1( float SIMD_SP_tiny, 1e-10f );
+ALIGN4_INIT1( float SIMD_SP_PI, idMath::PI );
+ALIGN4_INIT1( float SIMD_SP_halfPI, idMath::HALF_PI );
+ALIGN4_INIT1( float SIMD_SP_twoPI, idMath::TWO_PI );
+ALIGN4_INIT1( float SIMD_SP_oneOverTwoPI, 1.0f / idMath::TWO_PI );
+ALIGN4_INIT1( float SIMD_SP_infinity, idMath::INFINITY );
+
+
+/*
+============
+idSIMD_SSE2::GetName
+============
+*/
+const char * idSIMD_SSE2::GetName( void ) const {
+	return "MMX & SSE & SSE2";
+}
+
+#if 0		// the SSE2 code is ungodly slow
+
+/*
+============
+idSIMD_SSE2::MatX_LowerTriangularSolve
+
+  solves x in Lx = b for the n * n sub-matrix of L
+  if skip > 0 the first skip elements of x are assumed to be valid already
+  L has to be a lower triangular matrix with (implicit) ones on the diagonal
+  x == b is allowed
+============
+*/
+void VPCALL idSIMD_SSE2::MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip ) {
+	int nc;
+	const float *lptr;
+
+	if ( skip >= n ) {
+		return;
+	}
+
+	lptr = L[skip];
+	nc = L.GetNumColumns();
+
+	// unrolled cases for n < 8
+	if ( n < 8 ) {
+		#define NSKIP( n, s )	((n<<3)|(s&7))
+		switch( NSKIP( n, skip ) ) {
+			case NSKIP( 1, 0 ): x[0] = b[0];
+				return;
+			case NSKIP( 2, 0 ): x[0] = b[0];
+			case NSKIP( 2, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+				return;
+			case NSKIP( 3, 0 ): x[0] = b[0];
+			case NSKIP( 3, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+			case NSKIP( 3, 2 ): x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+				return;
+			case NSKIP( 4, 0 ): x[0] = b[0];
+			case NSKIP( 4, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+			case NSKIP( 4, 2 ): x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+			case NSKIP( 4, 3 ): x[3] = b[3] - lptr[3*nc+0] * x[0] - lptr[3*nc+1] * x[1] - lptr[3*nc+2] * x[2];
+				return;
+			case NSKIP( 5, 0 ): x[0] = b[0];
+			case NSKIP( 5, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+			case NSKIP( 5, 2 ): x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+			case NSKIP( 5, 3 ): x[3] = b[3] - lptr[3*nc+0] * x[0] - lptr[3*nc+1] * x[1] - lptr[3*nc+2] * x[2];
+			case NSKIP( 5, 4 ): x[4] = b[4] - lptr[4*nc+0] * x[0] - lptr[4*nc+1] * x[1] - lptr[4*nc+2] * x[2] - lptr[4*nc+3] * x[3];
+				return;
+			case NSKIP( 6, 0 ): x[0] = b[0];
+			case NSKIP( 6, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+			case NSKIP( 6, 2 ): x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+			case NSKIP( 6, 3 ): x[3] = b[3] - lptr[3*nc+0] * x[0] - lptr[3*nc+1] * x[1] - lptr[3*nc+2] * x[2];
+			case NSKIP( 6, 4 ): x[4] = b[4] - lptr[4*nc+0] * x[0] - lptr[4*nc+1] * x[1] - lptr[4*nc+2] * x[2] - lptr[4*nc+3] * x[3];
+			case NSKIP( 6, 5 ): x[5] = b[5] - lptr[5*nc+0] * x[0] - lptr[5*nc+1] * x[1] - lptr[5*nc+2] * x[2] - lptr[5*nc+3] * x[3] - lptr[5*nc+4] * x[4];
+				return;
+			case NSKIP( 7, 0 ): x[0] = b[0];
+			case NSKIP( 7, 1 ): x[1] = b[1] - lptr[1*nc+0] * x[0];
+			case NSKIP( 7, 2 ): x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+			case NSKIP( 7, 3 ): x[3] = b[3] - lptr[3*nc+0] * x[0] - lptr[3*nc+1] * x[1] - lptr[3*nc+2] * x[2];
+			case NSKIP( 7, 4 ): x[4] = b[4] - lptr[4*nc+0] * x[0] - lptr[4*nc+1] * x[1] - lptr[4*nc+2] * x[2] - lptr[4*nc+3] * x[3];
+			case NSKIP( 7, 5 ): x[5] = b[5] - lptr[5*nc+0] * x[0] - lptr[5*nc+1] * x[1] - lptr[5*nc+2] * x[2] - lptr[5*nc+3] * x[3] - lptr[5*nc+4] * x[4];
+			case NSKIP( 7, 6 ): x[6] = b[6] - lptr[6*nc+0] * x[0] - lptr[6*nc+1] * x[1] - lptr[6*nc+2] * x[2] - lptr[6*nc+3] * x[3] - lptr[6*nc+4] * x[4] - lptr[6*nc+5] * x[5];
+				return;
+		}
+		return;
+	}
+
+	// process first 4 rows
+	switch( skip ) {
+		case 0: x[0] = b[0];
+		case 1: x[1] = b[1] - lptr[1*nc+0] * x[0];
+		case 2: x[2] = b[2] - lptr[2*nc+0] * x[0] - lptr[2*nc+1] * x[1];
+		case 3: x[3] = b[3] - lptr[3*nc+0] * x[0] - lptr[3*nc+1] * x[1] - lptr[3*nc+2] * x[2];
+				skip = 4;
+	}
+
+	lptr = L[skip];
+
+	__asm {
+		push		ebx
+		mov			eax, skip				// eax = i
+		shl			eax, 2					// eax = i*4
+		mov			edx, n					// edx = n
+		shl			edx, 2					// edx = n*4
+		mov			esi, x					// esi = x
+		mov			edi, lptr				// edi = lptr
+		add			esi, eax
+		add			edi, eax
+		mov			ebx, b					// ebx = b
+		// aligned
+	looprow:
+		mov			ecx, eax
+		neg			ecx
+		cvtps2pd	xmm0, [esi+ecx]
+		cvtps2pd	xmm2, [edi+ecx]
+		mulpd		xmm0, xmm2
+		cvtps2pd	xmm1, [esi+ecx+8]
+		cvtps2pd	xmm3, [edi+ecx+8]
+		mulpd		xmm1, xmm3
+		add			ecx, 20*4
+		jg			donedot16
+	dot16:
+		cvtps2pd	xmm2, [esi+ecx-(16*4)]
+		cvtps2pd	xmm3, [edi+ecx-(16*4)]
+		cvtps2pd	xmm4, [esi+ecx-(14*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(14*4)]
+		addpd		xmm0, xmm2
+		cvtps2pd	xmm2, [esi+ecx-(12*4)]
+		mulpd		xmm4, xmm5
+		cvtps2pd	xmm3, [edi+ecx-(12*4)]
+		addpd		xmm1, xmm4
+		cvtps2pd	xmm4, [esi+ecx-(10*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(10*4)]
+		addpd		xmm0, xmm2
+		cvtps2pd	xmm2, [esi+ecx-(8*4)]
+		mulpd		xmm4, xmm5
+		cvtps2pd	xmm3, [edi+ecx-(8*4)]
+		addpd		xmm1, xmm4
+		cvtps2pd	xmm4, [esi+ecx-(6*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(6*4)]
+		addpd		xmm0, xmm2
+		cvtps2pd	xmm2, [esi+ecx-(4*4)]
+		mulpd		xmm4, xmm5
+		cvtps2pd	xmm3, [edi+ecx-(4*4)]
+		addpd		xmm1, xmm4
+		cvtps2pd	xmm4, [esi+ecx-(2*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(2*4)]
+		addpd		xmm0, xmm2
+		add			ecx, 16*4
+		mulpd		xmm4, xmm5
+		addpd		xmm1, xmm4
+		jle			dot16
+	donedot16:
+		sub			ecx, 8*4
+		jg			donedot8
+	dot8:
+		cvtps2pd	xmm2, [esi+ecx-(8*4)]
+		cvtps2pd	xmm3, [edi+ecx-(8*4)]
+		cvtps2pd	xmm7, [esi+ecx-(6*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(6*4)]
+		addpd		xmm0, xmm2
+		cvtps2pd	xmm6, [esi+ecx-(4*4)]
+		mulpd		xmm7, xmm5
+		cvtps2pd	xmm3, [edi+ecx-(4*4)]
+		addpd		xmm1, xmm7
+		cvtps2pd	xmm4, [esi+ecx-(2*4)]
+		mulpd		xmm6, xmm3
+		cvtps2pd	xmm7, [edi+ecx-(2*4)]
+		addpd		xmm0, xmm6
+		add			ecx, 8*4
+		mulpd		xmm4, xmm7
+		addpd		xmm1, xmm4
+	donedot8:
+		sub			ecx, 4*4
+		jg			donedot4
+	dot4:
+		cvtps2pd	xmm2, [esi+ecx-(4*4)]
+		cvtps2pd	xmm3, [edi+ecx-(4*4)]
+		cvtps2pd	xmm4, [esi+ecx-(2*4)]
+		mulpd		xmm2, xmm3
+		cvtps2pd	xmm5, [edi+ecx-(2*4)]
+		addpd		xmm0, xmm2
+		add			ecx, 4*4
+		mulpd		xmm4, xmm5
+		addpd		xmm1, xmm4
+	donedot4:
+		addpd		xmm0, xmm1
+		movaps		xmm1, xmm0
+		shufpd		xmm1, xmm1, R_SHUFFLEPD( 1, 0 )
+		addsd		xmm0, xmm1
+		sub			ecx, 4*4
+		jz			dot0
+		add			ecx, 4
+		jz			dot1
+		add			ecx, 4
+		jz			dot2
+	//dot3:
+		cvtss2sd	xmm1, [esi-(3*4)]
+		cvtss2sd	xmm2, [edi-(3*4)]
+		mulsd		xmm1, xmm2
+		addsd		xmm0, xmm1
+	dot2:
+		cvtss2sd	xmm3, [esi-(2*4)]
+		cvtss2sd	xmm4, [edi-(2*4)]
+		mulsd		xmm3, xmm4
+		addsd		xmm0, xmm3
+	dot1:
+		cvtss2sd	xmm5, [esi-(1*4)]
+		cvtss2sd	xmm6, [edi-(1*4)]
+		mulsd		xmm5, xmm6
+		addsd		xmm0, xmm5
+	dot0:
+		cvtss2sd	xmm1, [ebx+eax]
+		subsd		xmm1, xmm0
+		cvtsd2ss	xmm0, xmm1
+		movss		[esi], xmm0
+		add			eax, 4
+		cmp			eax, edx
+		jge			done
+		add			esi, 4
+		mov			ecx, nc
+		shl			ecx, 2
+		add			edi, ecx
+		add			edi, 4
+		jmp			looprow
+		// done
+	done:
+		pop			ebx
+	}
+}
+
+/*
+============
+idSIMD_SSE2::MatX_LowerTriangularSolveTranspose
+
+  solves x in L'x = b for the n * n sub-matrix of L
+  L has to be a lower triangular matrix with (implicit) ones on the diagonal
+  x == b is allowed
+============
+*/
+void VPCALL idSIMD_SSE2::MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n ) {
+	int nc;
+	const float *lptr;
+
+	lptr = L.ToFloatPtr();
+	nc = L.GetNumColumns();
+
+	// unrolled cases for n < 8
+	if ( n < 8 ) {
+		switch( n ) {
+			case 0:
+				return;
+			case 1:
+				x[0] = b[0];
+				return;
+			case 2:
+				x[1] = b[1];
+				x[0] = b[0] - lptr[1*nc+0] * x[1];
+				return;
+			case 3:
+				x[2] = b[2];
+				x[1] = b[1] - lptr[2*nc+1] * x[2];
+				x[0] = b[0] - lptr[2*nc+0] * x[2] - lptr[1*nc+0] * x[1];
+				return;
+			case 4:
+				x[3] = b[3];
+				x[2] = b[2] - lptr[3*nc+2] * x[3];
+				x[1] = b[1] - lptr[3*nc+1] * x[3] - lptr[2*nc+1] * x[2];
+				x[0] = b[0] - lptr[3*nc+0] * x[3] - lptr[2*nc+0] * x[2] - lptr[1*nc+0] * x[1];
+				return;
+			case 5:
+				x[4] = b[4];
+				x[3] = b[3] - lptr[4*nc+3] * x[4];
+				x[2] = b[2] - lptr[4*nc+2] * x[4] - lptr[3*nc+2] * x[3];
+				x[1] = b[1] - lptr[4*nc+1] * x[4] - lptr[3*nc+1] * x[3] - lptr[2*nc+1] * x[2];
+				x[0] = b[0] - lptr[4*nc+0] * x[4] - lptr[3*nc+0] * x[3] - lptr[2*nc+0] * x[2] - lptr[1*nc+0] * x[1];
+				return;
+			case 6:
+				x[5] = b[5];
+				x[4] = b[4] - lptr[5*nc+4] * x[5];
+				x[3] = b[3] - lptr[5*nc+3] * x[5] - lptr[4*nc+3] * x[4];
+				x[2] = b[2] - lptr[5*nc+2] * x[5] - lptr[4*nc+2] * x[4] - lptr[3*nc+2] * x[3];
+				x[1] = b[1] - lptr[5*nc+1] * x[5] - lptr[4*nc+1] * x[4] - lptr[3*nc+1] * x[3] - lptr[2*nc+1] * x[2];
+				x[0] = b[0] - lptr[5*nc+0] * x[5] - lptr[4*nc+0] * x[4] - lptr[3*nc+0] * x[3] - lptr[2*nc+0] * x[2] - lptr[1*nc+0] * x[1];
+				return;
+			case 7:
+				x[6] = b[6];
+				x[5] = b[5] - lptr[6*nc+5] * x[6];
+				x[4] = b[4] - lptr[6*nc+4] * x[6] - lptr[5*nc+4] * x[5];
+				x[3] = b[3] - lptr[6*nc+3] * x[6] - lptr[5*nc+3] * x[5] - lptr[4*nc+3] * x[4];
+				x[2] = b[2] - lptr[6*nc+2] * x[6] - lptr[5*nc+2] * x[5] - lptr[4*nc+2] * x[4] - lptr[3*nc+2] * x[3];
+				x[1] = b[1] - lptr[6*nc+1] * x[6] - lptr[5*nc+1] * x[5] - lptr[4*nc+1] * x[4] - lptr[3*nc+1] * x[3] - lptr[2*nc+1] * x[2];
+				x[0] = b[0] - lptr[6*nc+0] * x[6] - lptr[5*nc+0] * x[5] - lptr[4*nc+0] * x[4] - lptr[3*nc+0] * x[3] - lptr[2*nc+0] * x[2] - lptr[1*nc+0] * x[1];
+				return;
+		}
+		return;
+	}
+
+	int i, j, m;
+	float *xptr;
+	double s0;
+
+	// if the number of columns is not a multiple of 2 we're screwed for alignment.
+	// however, if the number of columns is a multiple of 2 but the number of to be
+	// processed rows is not a multiple of 2 we can still run 8 byte aligned
+	m = n;
+	if ( m & 1 ) {
+		m--;
+		x[m] = b[m];
+
+		lptr = L[m] + m - 4;
+		xptr = x + m;
+		__asm {
+			push		ebx
+			mov			eax, m					// eax = i
+			mov			esi, xptr				// esi = xptr
+			mov			edi, lptr				// edi = lptr
+			mov			ebx, b					// ebx = b
+			mov			edx, nc					// edx = nc*sizeof(float)
+			shl			edx, 2
+		process4rows_1:
+			cvtps2pd	xmm0, [ebx+eax*4-16]	// load b[i-2], b[i-1]
+			cvtps2pd	xmm2, [ebx+eax*4-8]		// load b[i-4], b[i-3]
+			xor			ecx, ecx
+			sub			eax, m
+			neg			eax
+			jz			done4x4_1
+		process4x4_1:	// process 4x4 blocks
+			cvtps2pd	xmm3, [edi]
+			cvtps2pd	xmm4, [edi+8]
+			add			edi, edx
+			cvtss2sd	xmm5, [esi+4*ecx+0]
+			shufpd		xmm5, xmm5, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm3, xmm5
+			cvtps2pd	xmm1, [edi]
+			mulpd		xmm4, xmm5
+			cvtps2pd	xmm6, [edi+8]
+			subpd		xmm0, xmm3
+			subpd		xmm2, xmm4
+			add			edi, edx
+			cvtss2sd	xmm7, [esi+4*ecx+4]
+			shufpd		xmm7, xmm7, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm1, xmm7
+			cvtps2pd	xmm3, [edi]
+			mulpd		xmm6, xmm7
+			cvtps2pd	xmm4, [edi+8]
+			subpd		xmm0, xmm1
+			subpd		xmm2, xmm6
+			add			edi, edx
+			cvtss2sd	xmm5, [esi+4*ecx+8]
+			shufpd		xmm5, xmm5, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm3, xmm5
+			cvtps2pd	xmm1, [edi]
+			mulpd		xmm4, xmm5
+			cvtps2pd	xmm6, [edi+8]
+			subpd		xmm0, xmm3
+			subpd		xmm2, xmm4
+			add			edi, edx
+			cvtss2sd	xmm7, [esi+4*ecx+12]
+			shufpd		xmm7, xmm7, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm1, xmm7
+			add			ecx, 4
+			mulpd		xmm6, xmm7
+			cmp			ecx, eax
+			subpd		xmm0, xmm1
+			subpd		xmm2, xmm6
+			jl			process4x4_1
+		done4x4_1:		// process left over of the 4 rows
+			cvtps2pd	xmm3, [edi]
+			cvtps2pd	xmm4, [edi+8]
+			cvtss2sd	xmm5, [esi+4*ecx]
+			shufpd		xmm5, xmm5, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm3, xmm5
+			mulpd		xmm4, xmm5
+			subpd		xmm0, xmm3
+			subpd		xmm2, xmm4
+			imul		ecx, edx
+			sub			edi, ecx
+			neg			eax
+
+			add			eax, m
+			sub			eax, 4
+			movapd		xmm1, xmm0
+			shufpd		xmm1, xmm1, R_SHUFFLEPD( 1, 1 )
+			movapd		xmm3, xmm2
+			shufpd		xmm3, xmm3, R_SHUFFLEPD( 1, 1 )
+			sub			edi, edx
+			cvtsd2ss	xmm7, xmm3
+			movss		[esi-4], xmm7			// xptr[-1] = s3
+			movsd		xmm4, xmm3
+			movsd		xmm5, xmm3
+			cvtss2sd	xmm7, [edi+8]
+			mulsd		xmm3, xmm7				// lptr[-1*nc+2] * s3
+			cvtss2sd	xmm7, [edi+4]
+			mulsd		xmm4, xmm7				// lptr[-1*nc+1] * s3
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm5, xmm7				// lptr[-1*nc+0] * s3
+			subsd		xmm2, xmm3
+			cvtsd2ss	xmm7, xmm2
+			movss		[esi-8], xmm7			// xptr[-2] = s2
+			movsd		xmm6, xmm2
+			sub			edi, edx
+			subsd		xmm0, xmm5
+			subsd		xmm1, xmm4
+			cvtss2sd	xmm7, [edi+4]
+			mulsd		xmm2, xmm7				// lptr[-2*nc+1] * s2
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm6, xmm7				// lptr[-2*nc+0] * s2
+			subsd		xmm1, xmm2
+			cvtsd2ss	xmm7, xmm1
+			movss		[esi-12], xmm7			// xptr[-3] = s1
+			subsd		xmm0, xmm6
+			sub			edi, edx
+			cmp			eax, 4
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm1, xmm7				// lptr[-3*nc+0] * s1
+			subsd		xmm0, xmm1
+			cvtsd2ss	xmm7, xmm0
+			movss		[esi-16], xmm7			// xptr[-4] = s0
+			jl			done4rows_1
+			sub			edi, edx
+			sub			edi, 16
+			sub			esi, 16
+			jmp			process4rows_1
+		done4rows_1:
+			pop			ebx
+		}
+	}
+	else {
+		lptr = L.ToFloatPtr() + m * L.GetNumColumns() + m - 4;
+		xptr = x + m;
+		__asm {
+			push		ebx
+			mov			eax, m					// eax = i
+			mov			esi, xptr				// esi = xptr
+			mov			edi, lptr				// edi = lptr
+			mov			ebx, b					// ebx = b
+			mov			edx, nc					// edx = nc*sizeof(float)
+			shl			edx, 2
+		process4rows:
+			cvtps2pd	xmm0, [ebx+eax*4-16]	// load b[i-2], b[i-1]
+			cvtps2pd	xmm2, [ebx+eax*4-8]		// load b[i-4], b[i-3]
+			sub			eax, m
+			jz			done4x4
+			neg			eax
+			xor			ecx, ecx
+		process4x4:		// process 4x4 blocks
+			cvtps2pd	xmm3, [edi]
+			cvtps2pd	xmm4, [edi+8]
+			add			edi, edx
+			cvtss2sd	xmm5, [esi+4*ecx+0]
+			shufpd		xmm5, xmm5, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm3, xmm5
+			cvtps2pd	xmm1, [edi]
+			mulpd		xmm4, xmm5
+			cvtps2pd	xmm6, [edi+8]
+			subpd		xmm0, xmm3
+			subpd		xmm2, xmm4
+			add			edi, edx
+			cvtss2sd	xmm7, [esi+4*ecx+4]
+			shufpd		xmm7, xmm7, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm1, xmm7
+			cvtps2pd	xmm3, [edi]
+			mulpd		xmm6, xmm7
+			cvtps2pd	xmm4, [edi+8]
+			subpd		xmm0, xmm1
+			subpd		xmm2, xmm6
+			add			edi, edx
+			cvtss2sd	xmm5, [esi+4*ecx+8]
+			shufpd		xmm5, xmm5, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm3, xmm5
+			cvtps2pd	xmm1, [edi]
+			mulpd		xmm4, xmm5
+			cvtps2pd	xmm6, [edi+8]
+			subpd		xmm0, xmm3
+			subpd		xmm2, xmm4
+			add			edi, edx
+			cvtss2sd	xmm7, [esi+4*ecx+12]
+			shufpd		xmm7, xmm7, R_SHUFFLEPD( 0, 0 )
+			mulpd		xmm1, xmm7
+			add			ecx, 4
+			mulpd		xmm6, xmm7
+			cmp			ecx, eax
+			subpd		xmm0, xmm1
+			subpd		xmm2, xmm6
+			jl			process4x4
+			imul		ecx, edx
+			sub			edi, ecx
+			neg			eax
+		done4x4:		// process left over of the 4 rows
+			add			eax, m
+			sub			eax, 4
+			movapd		xmm1, xmm0
+			shufpd		xmm1, xmm1, R_SHUFFLEPD( 1, 1 )
+			movapd		xmm3, xmm2
+			shufpd		xmm3, xmm3, R_SHUFFLEPD( 1, 1 )
+			sub			edi, edx
+			cvtsd2ss	xmm7, xmm3
+			movss		[esi-4], xmm7			// xptr[-1] = s3
+			movsd		xmm4, xmm3
+			movsd		xmm5, xmm3
+			cvtss2sd	xmm7, [edi+8]
+			mulsd		xmm3, xmm7				// lptr[-1*nc+2] * s3
+			cvtss2sd	xmm7, [edi+4]
+			mulsd		xmm4, xmm7				// lptr[-1*nc+1] * s3
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm5, xmm7				// lptr[-1*nc+0] * s3
+			subsd		xmm2, xmm3
+			cvtsd2ss	xmm7, xmm2
+			movss		[esi-8], xmm7			// xptr[-2] = s2
+			movsd		xmm6, xmm2
+			sub			edi, edx
+			subsd		xmm0, xmm5
+			subsd		xmm1, xmm4
+			cvtss2sd	xmm7, [edi+4]
+			mulsd		xmm2, xmm7				// lptr[-2*nc+1] * s2
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm6, xmm7				// lptr[-2*nc+0] * s2
+			subsd		xmm1, xmm2
+			cvtsd2ss	xmm7, xmm1
+			movss		[esi-12], xmm7			// xptr[-3] = s1
+			subsd		xmm0, xmm6
+			sub			edi, edx
+			cmp			eax, 4
+			cvtss2sd	xmm7, [edi]
+			mulsd		xmm1, xmm7				// lptr[-3*nc+0] * s1
+			subsd		xmm0, xmm1
+			cvtsd2ss	xmm7, xmm0
+			movss		[esi-16], xmm7			// xptr[-4] = s0
+			jl			done4rows
+			sub			edi, edx
+			sub			edi, 16
+			sub			esi, 16
+			jmp			process4rows
+		done4rows:
+			pop			ebx
+		}
+	}
+
+	// process left over rows
+	for ( i = (m&3)-1; i >= 0; i-- ) {
+		s0 = b[i];
+		lptr = L[i+1] + i;
+		for ( j = i + 1; j < m; j++ ) {
+			s0 -= lptr[0] * x[j];
+			lptr += nc;
+		}
+		x[i] = s0;
+	}
+}
+
+#endif
+
+/*
+============
+idSIMD_SSE2::MixedSoundToSamples
+============
+*/
+void VPCALL idSIMD_SSE2::MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples ) {
+
+	assert( ( numSamples % MIXBUFFER_SAMPLES ) == 0 );
+
+	__asm {
+
+		mov			eax, numSamples
+		mov			edi, mixBuffer
+		mov			esi, samples
+		shl			eax, 2
+		add			edi, eax
+		neg			eax
+
+	loop16:
+
+		movaps		xmm0, [edi+eax+0*16]
+		movaps		xmm1, [edi+eax+1*16]
+		movaps		xmm2, [edi+eax+2*16]
+		movaps		xmm3, [edi+eax+3*16]
+
+		add			esi, 4*4*2
+
+		cvtps2dq	xmm4, xmm0
+		cvtps2dq	xmm5, xmm1
+		cvtps2dq	xmm6, xmm2
+		cvtps2dq	xmm7, xmm3
+
+		prefetchnta	[edi+eax+128]
+
+		packssdw	xmm4, xmm5
+		packssdw	xmm6, xmm7
+
+		add			eax, 4*16
+
+		movlps		[esi-4*4*2], xmm4		// FIXME: should not use movlps/movhps to move integer data
+		movhps		[esi-3*4*2], xmm4
+		movlps		[esi-2*4*2], xmm6
+		movhps		[esi-1*4*2], xmm6
+
+		jl			loop16
+	}
+}
+
+#endif /* _WIN32 */

neo/idlib/math/Simd_SSE2.h

@@ -0,0 +1,57 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_SSE2_H__
+#define __MATH_SIMD_SSE2_H__
+
+/*
+===============================================================================
+
+	SSE2 implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_SSE2 : public idSIMD_SSE {
+public:
+#if defined(MACOS_X) && defined(__i386__)
+	virtual const char * VPCALL GetName( void ) const;
+	virtual void VPCALL CmpLT( byte *dst,			const byte bitNum,		const float *src0,		const float constant,	const int count );		
+
+#elif defined(_WIN32)
+	virtual const char * VPCALL GetName( void ) const;
+
+	//virtual void VPCALL MatX_LowerTriangularSolve( const idMatX &L, float *x, const float *b, const int n, int skip = 0 );
+	//virtual void VPCALL MatX_LowerTriangularSolveTranspose( const idMatX &L, float *x, const float *b, const int n );
+
+	virtual void VPCALL MixedSoundToSamples( short *samples, const float *mixBuffer, const int numSamples );
+
+#endif
+};
+
+#endif /* !__MATH_SIMD_SSE2_H__ */

neo/idlib/math/Simd_SSE3.cpp

@@ -0,0 +1,367 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+#include "Simd_Generic.h"
+#include "Simd_MMX.h"
+#include "Simd_SSE.h"
+#include "Simd_SSE2.h"
+#include "Simd_SSE3.h"
+
+
+//===============================================================
+//
+//	SSE3 implementation of idSIMDProcessor
+//
+//===============================================================
+
+#if defined(MACOS_X) && defined(__i386__)
+
+/*
+============
+idSIMD_SSE3::GetName
+============
+*/
+const char * idSIMD_SSE3::GetName( void ) const {
+	return "MMX & SSE & SSE2 & SSE3";
+}
+
+#elif defined(_WIN32)
+
+#include <xmmintrin.h>
+
+#define SHUFFLEPS( x, y, z, w )		(( (x) & 3 ) << 6 | ( (y) & 3 ) << 4 | ( (z) & 3 ) << 2 | ( (w) & 3 ))
+#define R_SHUFFLEPS( x, y, z, w )	(( (w) & 3 ) << 6 | ( (z) & 3 ) << 4 | ( (y) & 3 ) << 2 | ( (x) & 3 ))
+#define SHUFFLEPD( x, y )			(( (x) & 1 ) << 1 | ( (y) & 1 ))
+#define R_SHUFFLEPD( x, y )			(( (y) & 1 ) << 1 | ( (x) & 1 ))
+
+/*
+
+	The first argument of an instruction macro is the destination
+	and the second argument is the source operand. The destination
+	operand can be _xmm0 to _xmm7 only. The source operand can be
+	any one of the registers _xmm0 to _xmm7 or _eax, _ecx, _edx, _esp,
+	_ebp, _ebx, _esi, or _edi that contains the effective address.
+
+	For instance:  haddps   xmm0, xmm1
+	becomes:       haddps( _xmm0, _xmm1 )
+	and:           haddps   xmm0, [esi]
+	becomes:       haddps( _xmm0, _esi )
+
+	The ADDRESS_ADDC macro can be used when the effective source address
+	is formed by adding a constant to a general purpose register.
+	For instance:  haddps   xmm0, [esi+48]
+	becomes:       haddps( _xmm0, ADDRESS_ADDC( _esi, 48 ) )
+
+	The ADDRESS_ADDR macro can be used when the effective source address
+	is formed by adding two general purpose registers.
+	For instance:  haddps   xmm0, [esi+eax]
+	becomes:       haddps( _xmm0, ADDRESS_ADDR( _esi, _eax ) )
+
+	The ADDRESS_ADDRC macro can be used when the effective source address
+	is formed by adding two general purpose registers and a constant.
+	The constant must be in the range [-128, 127].
+	For instance:  haddps   xmm0, [esi+eax+48]
+	becomes:       haddps( _xmm0, ADDRESS_ADDRC( _esi, _eax, 48 ) )
+
+	The ADDRESS_SCALEADDR macro can be used when the effective source address is formed
+	by adding a scaled general purpose register to another general purpose register.
+	The scale must be either 1, 2, 4 or 8.
+	For instance:  haddps   xmm0, [esi+eax*4]
+	becomes:       haddps( _xmm0, ADDRESS_SCALEADDR( _esi, _eax, 4 ) )
+
+	The ADDRESS_SCALEADDRC macro can be used when the effective source address is formed
+	by adding a scaled general purpose register to another general purpose register and
+	also adding a constant. The scale must be either 1, 2, 4 or 8. The constant must
+	be in the range [-128, 127].
+	For instance:  haddps   xmm0, [esi+eax*4+64]
+	becomes:       haddps( _xmm0, ADDRESS_SCALEADDRC( _esi, _eax, 4, 64 ) )
+
+*/
+
+#define _eax	0x00
+#define _ecx	0x01
+#define _edx	0x02
+#define _ebx	0x03
+#define _esp	0x04
+#define _ebp	0x05
+#define _esi	0x06
+#define _edi	0x07
+
+#define _xmm0	0xC0
+#define _xmm1	0xC1
+#define _xmm2	0xC2
+#define _xmm3	0xC3
+#define _xmm4	0xC4
+#define _xmm5	0xC5
+#define _xmm6	0xC6
+#define _xmm7	0xC7
+
+#define RSCALE( s )		( (s&2)<<5 ) | ( (s&4)<<5 ) | ( (s&8)<<3 ) | ( (s&8)<<4 )
+
+#define ADDRESS_ADDC( reg0, constant )						0x40 | ( reg0 & 7 )	\
+	_asm _emit constant
+
+#define ADDRESS_ADDR( reg0, reg1 )							0x04				\
+	_asm _emit ( ( reg1 & 7 ) << 3 ) | ( reg0 & 7 )
+
+#define ADDRESS_ADDRC( reg0, reg1, constant )				0x44				\
+	_asm _emit ( ( reg1 & 7 ) << 3 ) | ( reg0 & 7 )								\
+	_asm _emit constant
+
+#define ADDRESS_SCALEADDR( reg0, reg1, scale )				0x04				\
+	_asm _emit ( ( reg1 & 7 ) << 3 ) | ( reg0 & 7 ) | RSCALE( scale )
+
+#define ADDRESS_SCALEADDRC( reg0, reg1, scale, constant )	0x44				\
+	_asm _emit ( ( reg1 & 7 ) << 3 ) | ( reg0 & 7 ) | RSCALE( scale )			\
+	_asm _emit constant
+
+
+// Packed Single-FP Add/Subtract ( dst[0]=dst[0]+src[0], dst[1]=dst[1]-src[1], dst[2]=dst[2]+src[2], dst[3]=dst[3]-src[3] )
+#define addsubps( dst, src )						\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0xD0									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Packed Double-FP Add/Subtract ( dst[0]=dst[0]+src[0], dst[1]=dst[1]-src[1] )
+#define addsubpd( dst, src )						\
+	_asm _emit 0x66									\
+	_asm _emit 0x0F									\
+	_asm _emit 0xD0									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Packed Single-FP Horizontal Add ( dst[0]=dst[0]+dst[1], dst[1]=dst[2]+dst[3], dst[2]=src[0]+src[1], dst[3]=src[2]+src[3] )
+#define haddps( dst, src )							\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x7C									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Packed Double-FP Horizontal Add ( dst[0]=dst[0]+dst[1], dst[1]=src[0]+src[1] )
+#define haddpd( dst, src )							\
+	_asm _emit 0x66									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x7C									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Packed Single-FP Horizontal Subtract ( dst[0]=dst[0]-dst[1], dst[1]=dst[2]-dst[3], dst[2]=src[0]-src[1], dst[3]=src[2]-src[3] )
+#define hsubps( dst, src )							\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x7D									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Packed Double-FP Horizontal Subtract ( dst[0]=dst[0]-dst[1], dst[1]=src[0]-src[1] )
+#define hsubpd( dst, src )							\
+	_asm _emit 0x66									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x7D									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Move Packed Single-FP Low and Duplicate ( dst[0]=src[0], dst[1]=src[0], dst[2]=src[2], dst[3]=src[2] )
+#define movsldup( dst, src )						\
+	_asm _emit 0xF3									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x12									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Move One Double-FP Low and Duplicate ( dst[0]=src[0], dst[1]=src[0] )
+#define movdldup( dst, src )						\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x12									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Move Packed Single-FP High and Duplicate ( dst[0]=src[1], dst[1]=src[1], dst[2]=src[3], dst[3]=src[3] )
+#define movshdup( dst, src )						\
+	_asm _emit 0xF3									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x16									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Move One Double-FP High and Duplicate ( dst[0]=src[1], dst[1]=src[1] )
+#define movdhdup( dst, src )						\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0x16									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+// Load Unaligned Integer 128 bits
+#define lddqu( dst, src )							\
+	_asm _emit 0xF2									\
+	_asm _emit 0x0F									\
+	_asm _emit 0xF0									\
+	_asm _emit ( ( dst & 7 ) << 3 ) | src
+
+
+#define DRAWVERT_SIZE				60
+#define DRAWVERT_XYZ_OFFSET			(0*4)
+#define DRAWVERT_ST_OFFSET			(3*4)
+#define DRAWVERT_NORMAL_OFFSET		(5*4)
+#define DRAWVERT_TANGENT0_OFFSET	(8*4)
+#define DRAWVERT_TANGENT1_OFFSET	(11*4)
+#define DRAWVERT_COLOR_OFFSET		(14*4)
+
+#define JOINTQUAT_SIZE				(7*4)
+#define JOINTMAT_SIZE				(4*3*4)
+#define JOINTWEIGHT_SIZE			(4*4)
+
+
+/*
+============
+SSE3_Dot
+============
+*/
+float SSE3_Dot( const idVec4 &v1, const idVec4 &v2 ) {
+	float d;
+	__asm {
+		mov		esi, v1
+		mov		edi, v2
+		movaps	xmm0, [esi]
+		mulps	xmm0, [edi]
+		haddps(	_xmm0, _xmm0 )
+		haddps(	_xmm0, _xmm0 )
+		movss	d, xmm0
+	}
+	return d;
+}
+
+/*
+============
+idSIMD_SSE3::GetName
+============
+*/
+const char * idSIMD_SSE3::GetName( void ) const {
+	return "MMX & SSE & SSE2 & SSE3";
+}
+
+/*
+============
+idSIMD_SSE3::TransformVerts
+============
+*/
+void VPCALL idSIMD_SSE3::TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights ) {
+#if 1
+
+	assert( sizeof( idDrawVert ) == DRAWVERT_SIZE );
+	assert( (int)&((idDrawVert *)0)->xyz == DRAWVERT_XYZ_OFFSET );
+	assert( sizeof( idVec4 ) == JOINTWEIGHT_SIZE );
+	assert( sizeof( idJointMat ) == JOINTMAT_SIZE );
+
+	__asm
+	{
+		mov			eax, numVerts
+		test		eax, eax
+		jz			done
+		imul		eax, DRAWVERT_SIZE
+
+		mov			ecx, verts
+		mov			edx, index
+		mov			esi, weights
+		mov			edi, joints
+
+		add			ecx, eax
+		neg			eax
+
+	loopVert:
+		mov			ebx, [edx]
+		movaps		xmm2, [esi]
+		add			edx, 8
+		movaps		xmm0, xmm2
+		add			esi, JOINTWEIGHT_SIZE
+		movaps		xmm1, xmm2
+
+		mulps		xmm0, [edi+ebx+ 0]						// xmm0 = m0, m1, m2, t0
+		mulps		xmm1, [edi+ebx+16]						// xmm1 = m3, m4, m5, t1
+		mulps		xmm2, [edi+ebx+32]						// xmm2 = m6, m7, m8, t2
+
+		cmp			dword ptr [edx-4], 0
+
+		jne			doneWeight
+
+	loopWeight:
+		mov			ebx, [edx]
+		movaps		xmm5, [esi]
+		add			edx, 8
+		movaps		xmm3, xmm5
+		add			esi, JOINTWEIGHT_SIZE
+		movaps		xmm4, xmm5
+
+		mulps		xmm3, [edi+ebx+ 0]						// xmm3 = m0, m1, m2, t0
+		mulps		xmm4, [edi+ebx+16]						// xmm4 = m3, m4, m5, t1
+		mulps		xmm5, [edi+ebx+32]						// xmm5 = m6, m7, m8, t2
+
+		cmp			dword ptr [edx-4], 0
+
+		addps		xmm0, xmm3
+		addps		xmm1, xmm4
+		addps		xmm2, xmm5
+
+		je			loopWeight
+
+	doneWeight:
+		add			eax, DRAWVERT_SIZE
+
+		haddps(		_xmm0, _xmm1 )
+		haddps(		_xmm2, _xmm0 )
+
+		movhps		[ecx+eax-DRAWVERT_SIZE+0], xmm2
+
+		haddps(		_xmm2, _xmm2 )
+
+		movss		[ecx+eax-DRAWVERT_SIZE+8], xmm2
+
+		jl			loopVert
+	done:
+	}
+
+#else
+
+	int i, j;
+	const byte *jointsPtr = (byte *)joints;
+
+	for( j = i = 0; i < numVerts; i++ ) {
+		idVec3 v;
+
+		v = ( *(idJointMat *) ( jointsPtr + index[j*2+0] ) ) * weights[j];
+		while( index[j*2+1] == 0 ) {
+			j++;
+			v += ( *(idJointMat *) ( jointsPtr + index[j*2+0] ) ) * weights[j];
+		}
+		j++;
+
+		verts[i].xyz = v;
+	}
+
+#endif
+}
+
+#endif /* _WIN32 */

neo/idlib/math/Simd_SSE3.h

@@ -0,0 +1,53 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#ifndef __MATH_SIMD_SSE3_H__
+#define __MATH_SIMD_SSE3_H__
+
+/*
+===============================================================================
+
+	SSE3 implementation of idSIMDProcessor
+
+===============================================================================
+*/
+
+class idSIMD_SSE3 : public idSIMD_SSE2 {
+public:
+#if defined(MACOS_X) && defined(__i386__)
+	virtual const char * VPCALL GetName( void ) const;
+
+#elif defined(_WIN32)
+	virtual const char * VPCALL GetName( void ) const;
+
+	virtual void VPCALL TransformVerts( idDrawVert *verts, const int numVerts, const idJointMat *joints, const idVec4 *weights, const int *index, const int numWeights );
+
+#endif
+};
+
+#endif /* !__MATH_SIMD_SSE3_H__ */

neo/idlib/math/Vector.cpp

@@ -0,0 +1,397 @@
+/*
+===========================================================================
+
+Doom 3 GPL Source Code
+Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  
+
+Doom 3 Source Code 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 3 of the License, or
+(at your option) any later version.
+
+Doom 3 Source Code 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 Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
+
+If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
+
+===========================================================================
+*/
+
+#include "../precompiled.h"
+#pragma hdrstop
+
+idVec2 vec2_origin( 0.0f, 0.0f );
+idVec3 vec3_origin( 0.0f, 0.0f, 0.0f );
+idVec4 vec4_origin( 0.0f, 0.0f, 0.0f, 0.0f );
+idVec5 vec5_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );
+idVec6 vec6_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );
+idVec6 vec6_infinity( idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY );
+
+
+//===============================================================
+//
+//	idVec2
+//
+//===============================================================
+
+/*
+=============
+idVec2::ToString
+=============
+*/
+const char *idVec2::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=============
+Lerp
+
+Linearly inperpolates one vector to another.
+=============
+*/
+void idVec2::Lerp( const idVec2 &v1, const idVec2 &v2, const float l ) {
+	if ( l <= 0.0f ) {
+		(*this) = v1;
+	} else if ( l >= 1.0f ) {
+		(*this) = v2;
+	} else {
+		(*this) = v1 + l * ( v2 - v1 );
+	}
+}
+
+
+//===============================================================
+//
+//	idVec3
+//
+//===============================================================
+
+/*
+=============
+idVec3::ToYaw
+=============
+*/
+float idVec3::ToYaw( void ) const {
+	float yaw;
+	
+	if ( ( y == 0.0f ) && ( x == 0.0f ) ) {
+		yaw = 0.0f;
+	} else {
+		yaw = RAD2DEG( atan2( y, x ) );
+		if ( yaw < 0.0f ) {
+			yaw += 360.0f;
+		}
+	}
+
+	return yaw;
+}
+
+/*
+=============
+idVec3::ToPitch
+=============
+*/
+float idVec3::ToPitch( void ) const {
+	float	forward;
+	float	pitch;
+	
+	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
+		if ( z > 0.0f ) {
+			pitch = 90.0f;
+		} else {
+			pitch = 270.0f;
+		}
+	} else {
+		forward = ( float )idMath::Sqrt( x * x + y * y );
+		pitch = RAD2DEG( atan2( z, forward ) );
+		if ( pitch < 0.0f ) {
+			pitch += 360.0f;
+		}
+	}
+
+	return pitch;
+}
+
+/*
+=============
+idVec3::ToAngles
+=============
+*/
+idAngles idVec3::ToAngles( void ) const {
+	float forward;
+	float yaw;
+	float pitch;
+	
+	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
+		yaw = 0.0f;
+		if ( z > 0.0f ) {
+			pitch = 90.0f;
+		} else {
+			pitch = 270.0f;
+		}
+	} else {
+		yaw = RAD2DEG( atan2( y, x ) );
+		if ( yaw < 0.0f ) {
+			yaw += 360.0f;
+		}
+
+		forward = ( float )idMath::Sqrt( x * x + y * y );
+		pitch = RAD2DEG( atan2( z, forward ) );
+		if ( pitch < 0.0f ) {
+			pitch += 360.0f;
+		}
+	}
+
+	return idAngles( -pitch, yaw, 0.0f );
+}
+
+/*
+=============
+idVec3::ToPolar
+=============
+*/
+idPolar3 idVec3::ToPolar( void ) const {
+	float forward;
+	float yaw;
+	float pitch;
+	
+	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
+		yaw = 0.0f;
+		if ( z > 0.0f ) {
+			pitch = 90.0f;
+		} else {
+			pitch = 270.0f;
+		}
+	} else {
+		yaw = RAD2DEG( atan2( y, x ) );
+		if ( yaw < 0.0f ) {
+			yaw += 360.0f;
+		}
+
+		forward = ( float )idMath::Sqrt( x * x + y * y );
+		pitch = RAD2DEG( atan2( z, forward ) );
+		if ( pitch < 0.0f ) {
+			pitch += 360.0f;
+		}
+	}
+	return idPolar3( idMath::Sqrt( x * x + y * y + z * z ), yaw, -pitch );
+}
+
+/*
+=============
+idVec3::ToMat3
+=============
+*/
+idMat3 idVec3::ToMat3( void ) const {
+	idMat3	mat;
+	float	d;
+
+	mat[0] = *this;
+	d = x * x + y * y;
+	if ( !d ) {
+		mat[1][0] = 1.0f;
+		mat[1][1] = 0.0f;
+		mat[1][2] = 0.0f;
+	} else {
+		d = idMath::InvSqrt( d );
+		mat[1][0] = -y * d;
+		mat[1][1] = x * d;
+		mat[1][2] = 0.0f;
+	}
+	mat[2] = Cross( mat[1] );
+
+	return mat;
+}
+
+/*
+=============
+idVec3::ToString
+=============
+*/
+const char *idVec3::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=============
+Lerp
+
+Linearly inperpolates one vector to another.
+=============
+*/
+void idVec3::Lerp( const idVec3 &v1, const idVec3 &v2, const float l ) {
+	if ( l <= 0.0f ) {
+		(*this) = v1;
+	} else if ( l >= 1.0f ) {
+		(*this) = v2;
+	} else {
+		(*this) = v1 + l * ( v2 - v1 );
+	}
+}
+
+/*
+=============
+SLerp
+
+Spherical linear interpolation from v1 to v2.
+Vectors are expected to be normalized.
+=============
+*/
+#define LERP_DELTA 1e-6
+
+void idVec3::SLerp( const idVec3 &v1, const idVec3 &v2, const float t ) {
+	float omega, cosom, sinom, scale0, scale1;
+
+	if ( t <= 0.0f ) {
+		(*this) = v1;
+		return;
+	} else if ( t >= 1.0f ) {
+		(*this) = v2;
+		return;
+	}
+
+	cosom = v1 * v2;
+	if ( ( 1.0f - cosom ) > LERP_DELTA ) {
+		omega = acos( cosom );
+		sinom = sin( omega );
+		scale0 = sin( ( 1.0f - t ) * omega ) / sinom;
+		scale1 = sin( t * omega ) / sinom;
+	} else {
+		scale0 = 1.0f - t;
+		scale1 = t;
+	}
+
+	(*this) = ( v1 * scale0 + v2 * scale1 );
+}
+
+/*
+=============
+ProjectSelfOntoSphere
+
+Projects the z component onto a sphere.
+=============
+*/
+void idVec3::ProjectSelfOntoSphere( const float radius ) {
+	float rsqr = radius * radius;
+	float len = Length();
+	if ( len  < rsqr * 0.5f ) {
+		z = sqrt( rsqr - len );
+	} else {
+		z = rsqr / ( 2.0f * sqrt( len ) );
+	}
+}
+
+
+
+//===============================================================
+//
+//	idVec4
+//
+//===============================================================
+
+/*
+=============
+idVec4::ToString
+=============
+*/
+const char *idVec4::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=============
+Lerp
+
+Linearly inperpolates one vector to another.
+=============
+*/
+void idVec4::Lerp( const idVec4 &v1, const idVec4 &v2, const float l ) {
+	if ( l <= 0.0f ) {
+		(*this) = v1;
+	} else if ( l >= 1.0f ) {
+		(*this) = v2;
+	} else {
+		(*this) = v1 + l * ( v2 - v1 );
+	}
+}
+
+
+//===============================================================
+//
+//	idVec5
+//
+//===============================================================
+
+/*
+=============
+idVec5::ToString
+=============
+*/
+const char *idVec5::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=============
+idVec5::Lerp
+=============
+*/
+void idVec5::Lerp( const idVec5 &v1, const idVec5 &v2, const float l ) {
+	if ( l <= 0.0f ) {
+		(*this) = v1;
+	} else if ( l >= 1.0f ) {
+		(*this) = v2;
+	} else {
+		x = v1.x + l * ( v2.x - v1.x );
+		y = v1.y + l * ( v2.y - v1.y );
+		z = v1.z + l * ( v2.z - v1.z );
+		s = v1.s + l * ( v2.s - v1.s );
+		t = v1.t + l * ( v2.t - v1.t );
+	}
+}
+
+
+//===============================================================
+//
+//	idVec6
+//
+//===============================================================
+
+/*
+=============
+idVec6::ToString
+=============
+*/
+const char *idVec6::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+
+//===============================================================
+//
+//	idVecX
+//
+//===============================================================
+
+float	idVecX::temp[VECX_MAX_TEMP+4];
+float *	idVecX::tempPtr = (float *) ( ( (int) idVecX::temp + 15 ) & ~15 );
+int		idVecX::tempIndex = 0;
+
+/*
+=============
+idVecX::ToString
+=============
+*/
+const char *idVecX::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}