Initial commit

neo/idlib/math/Angles.cpp

@@ -0,0 +1,240 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+#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() {
+	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() {
+	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() 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() 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() 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() 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() const {
+	return ToMat3().ToMat4();
+}
+
+/*
+=================
+idAngles::ToAngularVelocity
+=================
+*/
+idVec3 idAngles::ToAngularVelocity() 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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+					idAngles( float pitch, float yaw, float roll );
+					explicit idAngles( const idVec3 &v );
+
+	void 			Set( float pitch, float yaw, float roll );
+	idAngles &		Zero();
+
+	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();	// normalizes 'this'
+	idAngles &		Normalize180();	// normalizes 'this'
+
+	void			Clamp( const idAngles &min, const idAngles &max );
+
+	int				GetDimension() const;
+
+	void			ToVectors( idVec3 *forward, idVec3 *right = NULL, idVec3 *up = NULL ) const;
+	idVec3			ToForward() const;
+	idQuat			ToQuat() const;
+	idRotation		ToRotation() const;
+	idMat3			ToMat3() const;
+	idMat4			ToMat4() const;
+	idVec3			ToAngularVelocity() const;
+	const float *	ToFloatPtr() const;
+	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+};
+
+extern idAngles ang_zero;
+
+ID_INLINE idAngles::idAngles() {
+}
+
+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() {
+	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() const {
+	return 3;
+}
+
+ID_INLINE const float *idAngles::ToFloatPtr() const {
+	return &pitch;
+}
+
+ID_INLINE float *idAngles::ToFloatPtr() {
+	return &pitch;
+}
+
+#endif /* !__MATH_ANGLES_H__ */

neo/idlib/math/Complex.cpp

@@ -0,0 +1,41 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+						idComplex( const float r, const float i );
+
+	void 				Set( const float r, const float i );
+	void				Zero();
+
+	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() const;
+	idComplex			Sqrt() const;
+	float				Abs() const;
+
+	int					GetDimension() const;
+
+	const float *		ToFloatPtr() const;
+	float *				ToFloatPtr();
+	const char *		ToString( int precision = 2 ) const;
+};
+
+extern idComplex complex_origin;
+#define complex_zero complex_origin
+
+ID_INLINE idComplex::idComplex() {
+}
+
+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() {
+	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() 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() 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() 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() const {
+	return 2;
+}
+
+ID_INLINE const float *idComplex::ToFloatPtr() const {
+	return &r;
+}
+
+ID_INLINE float *idComplex::ToFloatPtr() {
+	return &r;
+}
+
+#endif /* !__MATH_COMPLEX_H__ */

neo/idlib/math/Extrapolate.h

@@ -0,0 +1,219 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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 int startTime, const int duration, const type &startValue, const type &baseSpeed, const type &speed, const extrapolation_t extrapolationType );
+	type				GetCurrentValue( int time ) const;
+	type				GetCurrentSpeed( int time ) const;
+	bool				IsDone( int time ) const { return ( !( extrapolationType & EXTRAPOLATION_NOSTOP ) && time >= startTime + duration ); }
+	void				SetStartTime( int time ) { startTime = time; }
+	int					GetStartTime() const { return startTime; }
+	int					GetEndTime() const { return ( !( extrapolationType & EXTRAPOLATION_NOSTOP ) && duration > 0 ) ? startTime + duration : 0; }
+	int					GetDuration() const { return duration; }
+	void				SetStartValue( const type &value ) { startValue = value; }
+	const type &		GetStartValue() const { return startValue; }
+	const type &		GetBaseSpeed() const { return baseSpeed; }
+	const type &		GetSpeed() const { return speed; }
+	extrapolation_t		GetExtrapolationType() const { return extrapolationType; }
+
+private:
+	extrapolation_t		extrapolationType;
+	int					startTime;
+	int					duration;
+	type				startValue;
+	type				baseSpeed;
+	type				speed;
+};
+
+/*
+====================
+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 ) );
+}
+
+/*
+====================
+idExtrapolate::Init
+====================
+*/
+template< class type >
+ID_INLINE void idExtrapolate<type>::Init( const int startTime, const int 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;
+}
+
+/*
+====================
+idExtrapolate::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idExtrapolate<type>::GetCurrentValue( int time ) const {
+	if ( time < startTime ) {
+		return startValue;
+	}
+
+	if ( !( extrapolationType &	EXTRAPOLATION_NOSTOP ) && ( time > startTime + duration ) ) {
+		time = startTime + duration;
+	}
+
+	switch ( extrapolationType & ~EXTRAPOLATION_NOSTOP ) {
+		case EXTRAPOLATION_NONE: {
+			const float deltaTime = ( time - startTime ) * 0.001f;
+			return startValue + deltaTime * baseSpeed;
+		}
+		case EXTRAPOLATION_LINEAR: {
+			const float deltaTime = ( time - startTime ) * 0.001f;
+			return startValue + deltaTime * ( baseSpeed + speed );
+		}
+		case EXTRAPOLATION_ACCELLINEAR: {
+			if ( duration == 0 ) {
+				return startValue;
+			} else {
+				const float deltaTime = ( time - startTime ) / (float)duration;
+				const float s = ( 0.5f * deltaTime * deltaTime ) * ( (float)duration * 0.001f );
+				return startValue + deltaTime * baseSpeed + s * speed;
+			}
+		}
+		case EXTRAPOLATION_DECELLINEAR: {
+			if ( duration == 0 ) {
+				return startValue;
+			} else {
+				const float deltaTime = ( time - startTime ) / (float)duration;
+				const float s = ( deltaTime - ( 0.5f * deltaTime * deltaTime ) ) * ( (float)duration * 0.001f );
+				return startValue + deltaTime * baseSpeed + s * speed;
+			}
+		}
+		case EXTRAPOLATION_ACCELSINE: {
+			if ( duration == 0 ) {
+				return startValue;
+			} else {
+				const float deltaTime = ( time - startTime ) / (float)duration;
+				const float s = ( 1.0f - idMath::Cos( deltaTime * idMath::HALF_PI ) ) * (float)duration * 0.001f * idMath::SQRT_1OVER2;
+				return startValue + deltaTime * baseSpeed + s * speed;
+			}
+		}
+		case EXTRAPOLATION_DECELSINE: {
+			if ( duration == 0 ) {
+				return startValue;
+			} else {
+				const float deltaTime = ( time - startTime ) / (float)duration;
+				const float s = idMath::Sin( deltaTime * idMath::HALF_PI ) * (float)duration * 0.001f * idMath::SQRT_1OVER2;
+				return startValue + deltaTime * baseSpeed + s * speed;
+			}
+		}
+	}
+	return startValue;
+}
+
+/*
+====================
+idExtrapolate::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idExtrapolate<type>::GetCurrentSpeed( int time ) const {
+	if ( time < startTime || duration == 0 ) {
+		return ( startValue - startValue ); //-V501
+	}
+
+	if ( !( extrapolationType &	EXTRAPOLATION_NOSTOP ) && ( time > startTime + duration ) ) {
+		return ( startValue - startValue ); //-V501
+	}
+
+	switch( extrapolationType & ~EXTRAPOLATION_NOSTOP ) {
+		case EXTRAPOLATION_NONE: {
+			return baseSpeed;
+		}
+		case EXTRAPOLATION_LINEAR: {
+			return baseSpeed + speed;
+		}
+		case EXTRAPOLATION_ACCELLINEAR: {
+			const float deltaTime = ( time - startTime ) / (float)duration;
+			const float s = deltaTime;
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_DECELLINEAR: {
+			const float deltaTime = ( time - startTime ) / (float)duration;
+			const float s = 1.0f - deltaTime;
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_ACCELSINE: {
+			const float deltaTime = ( time - startTime ) / (float)duration;
+			const float s = idMath::Sin( deltaTime * idMath::HALF_PI );
+			return baseSpeed + s * speed;
+		}
+		case EXTRAPOLATION_DECELSINE: {
+			const float deltaTime = ( time - startTime ) / (float)duration;
+			const float 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,400 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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 int startTime, const int duration, const type &startValue, const type &endValue );
+	void				SetStartTime( int time ) { this->startTime = time; }
+	void				SetDuration( int duration ) { this->duration = duration; }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; }
+
+	type				GetCurrentValue( int time ) const;
+	bool				IsDone( int time ) const { return ( time >= startTime + duration ); }
+
+	int					GetStartTime() const { return startTime; }
+	int					GetEndTime() const { return startTime + duration; }
+	int					GetDuration() const { return duration; }
+	const type &		GetStartValue() const { return startValue; }
+	const type &		GetEndValue() const { return endValue; }
+
+private:
+	int					startTime;
+	int					duration;
+	type				startValue;
+	type				endValue;
+};
+
+/*
+====================
+idInterpolate::idInterpolate
+====================
+*/
+template< class type >
+ID_INLINE idInterpolate<type>::idInterpolate() {
+	startTime = duration = 0;
+	memset( &startValue, 0, sizeof( startValue ) );
+	memset( &endValue, 0, sizeof( endValue ) );
+}
+
+/*
+====================
+idInterpolate::Init
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolate<type>::Init( const int startTime, const int duration, const type &startValue, const type &endValue ) {
+	this->startTime = startTime;
+	this->duration = duration;
+	this->startValue = startValue;
+	this->endValue = endValue;
+}
+
+/*
+====================
+idInterpolate::GetCurrentValue
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolate<type>::GetCurrentValue( int time ) const {
+	if ( time <= startTime ) {
+		return startValue;
+	} else if ( time >= startTime + duration ) {
+		return endValue;
+	} else {
+		const float deltaTime = time - startTime;
+		const float f = deltaTime / (float)duration;
+		const type range = ( endValue - startValue );
+		return startValue + ( range * f );
+	}
+}
+
+/*
+==============================================================================================
+
+	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 int startTime, const int accelTime, const int decelTime, const int duration, const type &startValue, const type &endValue );
+	void				SetStartTime( int time ) { startTime = time; Invalidate(); }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; Invalidate(); }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; Invalidate(); }
+
+	type				GetCurrentValue( int time ) const;
+	type				GetCurrentSpeed( int time ) const;
+	bool				IsDone( int time ) const { return ( time >= startTime + accelTime + linearTime + decelTime ); }
+
+	int					GetStartTime() const { return startTime; }
+	int					GetEndTime() const { return startTime + accelTime + linearTime + decelTime; }
+	int					GetDuration() const { return accelTime + linearTime + decelTime; }
+	int					GetAcceleration() const { return accelTime; }
+	int					GetDeceleration() const { return decelTime; }
+	const type &		GetStartValue() const { return startValue; }
+	const type &		GetEndValue() const { return endValue; }
+
+private:
+	int					startTime;
+	int					accelTime;
+	int					linearTime;
+	int					decelTime;
+	type				startValue;
+	type				endValue;
+	mutable idExtrapolate<type> extrapolate;
+
+	void				Invalidate();
+	void				SetPhase( int 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 int startTime, const int accelTime, const int decelTime, const int duration, const type &startValue, const type &endValue ) {
+	this->startTime = startTime;
+	this->accelTime = accelTime;
+	this->decelTime = decelTime;
+	this->startValue = startValue;
+	this->endValue = endValue;
+
+	if ( duration <= 0 ) {
+		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;
+	const type 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 ); //-V501
+	} else if ( this->linearTime ) {
+		extrapolate.Init( startTime, this->linearTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_LINEAR ); //-V501
+	} else {
+		extrapolate.Init( startTime, this->decelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_DECELLINEAR ); //-V501
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::Invalidate
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelLinear<type>::Invalidate() {
+	extrapolate.Init( 0, 0, extrapolate.GetStartValue(), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_NONE );
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::SetPhase
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelLinear<type>::SetPhase( int time ) const {
+	const float 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( int time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentValue( time );
+}
+
+/*
+====================
+idInterpolateAccelDecelLinear::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelLinear<type>::GetCurrentSpeed( int 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 int startTime, const int accelTime, const int decelTime, const int duration, const type &startValue, const type &endValue );
+	void				SetStartTime( int time ) { startTime = time; Invalidate(); }
+	void				SetStartValue( const type &startValue ) { this->startValue = startValue; Invalidate(); }
+	void				SetEndValue( const type &endValue ) { this->endValue = endValue; Invalidate(); }
+
+	type				GetCurrentValue( int time ) const;
+	type				GetCurrentSpeed( int time ) const;
+	bool				IsDone( int time ) const { return ( time >= startTime + accelTime + linearTime + decelTime ); }
+
+	int					GetStartTime() const { return startTime; }
+	int					GetEndTime() const { return startTime + accelTime + linearTime + decelTime; }
+	int					GetDuration() const { return accelTime + linearTime + decelTime; }
+	int					GetAcceleration() const { return accelTime; }
+	int					GetDeceleration() const { return decelTime; }
+	const type &		GetStartValue() const { return startValue; }
+	const type &		GetEndValue() const { return endValue; }
+
+private:
+	int					startTime;
+	int					accelTime;
+	int					linearTime;
+	int					decelTime;
+	type				startValue;
+	type				endValue;
+	mutable idExtrapolate<type> extrapolate;
+
+	void				Invalidate();
+	void				SetPhase( int time ) const;
+};
+
+/*
+====================
+idInterpolateAccelDecelSine::idInterpolateAccelDecelSine
+====================
+*/
+template< class type >
+ID_INLINE idInterpolateAccelDecelSine<type>::idInterpolateAccelDecelSine() {
+	startTime = accelTime = linearTime = decelTime = 0;
+	memset( &startValue, 0, sizeof( startValue ) );
+	memset( &endValue, 0, sizeof( endValue ) );
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::Init
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::Init( const int startTime, const int accelTime, const int decelTime, const int duration, const type &startValue, const type &endValue ) {
+	this->startTime = startTime;
+	this->accelTime = accelTime;
+	this->decelTime = decelTime;
+	this->startValue = startValue;
+	this->endValue = endValue;
+
+	if ( duration <= 0 ) {
+		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;
+	const type 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 ); //-V501
+	} else if ( this->linearTime ) {
+		extrapolate.Init( startTime, this->linearTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_LINEAR ); //-V501
+	} else {
+		extrapolate.Init( startTime, this->decelTime, startValue, ( startValue - startValue ), speed, EXTRAPOLATION_DECELSINE ); //-V501
+	}
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::Invalidate
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::Invalidate() {
+	extrapolate.Init( 0, 0, extrapolate.GetStartValue(), extrapolate.GetBaseSpeed(), extrapolate.GetSpeed(), EXTRAPOLATION_NONE );
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::SetPhase
+====================
+*/
+template< class type >
+ID_INLINE void idInterpolateAccelDecelSine<type>::SetPhase( int time ) const {
+	const float 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( int time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentValue( time );
+}
+
+/*
+====================
+idInterpolateAccelDecelSine::GetCurrentSpeed
+====================
+*/
+template< class type >
+ID_INLINE type idInterpolateAccelDecelSine<type>::GetCurrentSpeed( int time ) const {
+	SetPhase( time );
+	return extrapolate.GetCurrentSpeed( time );
+}
+
+#endif /* !__MATH_INTERPOLATE_H__ */

neo/idlib/math/Lcp.h

@@ -0,0 +1,79 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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__
+
+/*
+================================================
+The *LCP* class, idLCP, is a 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:
+
+	lo[i] < x[i] < hi[i], t[i] == 0
+	x[i] == lo[i], t[i] >= 0
+	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();		// 'A' must be a square matrix
+	static idLCP *	AllocSymmetric();	// 'A' must be a symmetric matrix
+
+	virtual			~idLCP();
+
+	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();
+
+	static void		Test_f( const idCmdArgs &args );
+
+protected:
+	int				maxIterations;
+};
+
+#endif // !__MATH_LCP_H__

neo/idlib/math/Math.cpp

@@ -0,0 +1,147 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+const int SMALLEST_NON_DENORMAL					= 1<<IEEE_FLT_MANTISSA_BITS;
+const int NAN_VALUE								= 0x7f800000;
+
+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::ONEOVER_PI		= 1.0f / idMath::PI;
+const float idMath::ONEOVER_TWOPI	= 1.0f / idMath::TWO_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;
+const float idMath::FLT_SMALLEST_NON_DENORMAL	= * reinterpret_cast< const float * >( & SMALLEST_NON_DENORMAL );	// 1.1754944e-038f
+
+const __m128 idMath::SIMD_SP_zero				= { 0.0f, 0.0f, 0.0f, 0.0f };
+const __m128 idMath::SIMD_SP_255				= { 255.0f, 255.0f, 255.0f, 255.0f };
+const __m128 idMath::SIMD_SP_min_char			= { -128.0f, -128.0f, -128.0f, -128.0f };
+const __m128 idMath::SIMD_SP_max_char			= { 127.0f, 127.0f, 127.0f, 127.0f };
+const __m128 idMath::SIMD_SP_min_short			= { -32768.0f, -32768.0f, -32768.0f, -32768.0f };
+const __m128 idMath::SIMD_SP_max_short			= { 32767.0f, 32767.0f, 32767.0f, 32767.0f };
+const __m128 idMath::SIMD_SP_smallestNonDenorm	= { FLT_SMALLEST_NON_DENORMAL, FLT_SMALLEST_NON_DENORMAL, FLT_SMALLEST_NON_DENORMAL, FLT_SMALLEST_NON_DENORMAL };
+const __m128 idMath::SIMD_SP_tiny				= { 1e-4f, 1e-4f, 1e-4f, 1e-4f };
+const __m128 idMath::SIMD_SP_rsqrt_c0			= { 3.0f, 3.0f, 3.0f, 3.0f };
+const __m128 idMath::SIMD_SP_rsqrt_c1			= { -0.5f, -0.5f, -0.5f, -0.5f };
+
+bool		idMath::initialized		= false;
+dword		idMath::iSqrt[SQRT_TABLE_SIZE];		// inverse square root lookup table
+
+/*
+===============
+idMath::Init
+===============
+*/
+void idMath::Init() {
+    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 |= ( ( INT32_SIGNBITSET( 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/Ode.cpp

@@ -0,0 +1,355 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+//===============================================================
+//
+//	idODE_Euler
+//
+//===============================================================
+
+/*
+=============
+idODE_Euler::idODE_Euler
+=============
+*/
+idODE_Euler::idODE_Euler( const int dim, deriveFunction_t dr, const void *ud ) {
+	dimension = dim;
+	derivatives = new (TAG_MATH) float[dim];
+	derive = dr;
+	userData = ud;
+}
+
+/*
+=============
+idODE_Euler::~idODE_Euler
+=============
+*/
+idODE_Euler::~idODE_Euler() {
+	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 (TAG_MATH) float[dim];
+	derivatives = new (TAG_MATH) float[dim];
+	derive = dr;
+	userData = ud;
+}
+
+/*
+=============
+idODE_Midpoint::~idODE_Midpoint
+=============
+*/
+idODE_Midpoint::~idODE_Midpoint() {
+	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 (TAG_MATH) float[dim];
+	d1 = new (TAG_MATH) float[dim];
+	d2 = new (TAG_MATH) float[dim];
+	d3 = new (TAG_MATH) float[dim];
+	d4 = new (TAG_MATH) float[dim];
+}
+
+/*
+=============
+idODE_RK4::~idODE_RK4
+=============
+*/
+idODE_RK4::~idODE_RK4() {
+	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 (TAG_MATH) float[dim];
+	d1 = new (TAG_MATH) float[dim];
+	d1half = new (TAG_MATH) float [dim];
+	d2 = new (TAG_MATH) float[dim];
+	d3 = new (TAG_MATH) float[dim];
+	d4 = new (TAG_MATH) float[dim];
+}
+
+/*
+=============
+idODE_RK4Adaptive::~idODE_RK4Adaptive
+=============
+*/
+idODE_RK4Adaptive::~idODE_RK4Adaptive() {
+	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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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() {}
+
+	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();
+
+	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();
+
+	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();
+
+	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();
+
+	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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+idPlane plane_origin( 0.0f, 0.0f, 0.0f, 0.0f );
+
+/*
+================
+idPlane::Type
+================
+*/
+int idPlane::Type() 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,401 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+					explicit idPlane( float a, float b, float c, float d );
+					explicit idPlane( const idVec3 &normal, const float dist );
+					explicit idPlane( const idVec3 & v0, const idVec3 & v1, const idVec3 & v2, bool fixDegenerate = false );
+
+	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 float s ) const;		// scale plane
+	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();							// zero plane
+	void			SetNormal( const idVec3 &normal );		// sets the normal
+	const idVec3 &	Normal() const;					// reference to const normal
+	idVec3 &		Normal();							// reference to normal
+	float			Normalize( bool fixDegenerate = true );	// only normalizes the plane normal, does not adjust d
+	bool			FixDegenerateNormal();			// fix degenerate normal
+	bool			FixDegeneracies( float distEpsilon );	// fix degenerate normal and dist
+	float			Dist() const;						// returns: -d
+	void			SetDist( const float dist );			// sets: d = -dist
+	int				Type() 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() const;
+
+	const idVec4 &	ToVec4() const;
+	idVec4 &		ToVec4();
+	const float *	ToFloatPtr() const;
+	float *			ToFloatPtr();
+	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() {
+}
+
+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 idPlane::idPlane( const idVec3 & v0, const idVec3 & v1, const idVec3 & v2, bool fixDegenerate ) {
+	FromPoints( v0, v1, v2, fixDegenerate );
+}
+
+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 float s ) const {
+	return idPlane( a * s, b * s, c * s, d * s );
+}
+
+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() {
+	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() const {
+	return *reinterpret_cast<const idVec3 *>(&a);
+}
+
+ID_INLINE idVec3 &idPlane::Normal() {
+	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() {
+	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() 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() const {
+	return 4;
+}
+
+ID_INLINE const idVec4 &idPlane::ToVec4() const {
+	return *reinterpret_cast<const idVec4 *>(&a);
+}
+
+ID_INLINE idVec4 &idPlane::ToVec4() {
+	return *reinterpret_cast<idVec4 *>(&a);
+}
+
+ID_INLINE const float *idPlane::ToFloatPtr() const {
+	return reinterpret_cast<const float *>(&a);
+}
+
+ID_INLINE float *idPlane::ToFloatPtr() {
+	return reinterpret_cast<float *>(&a);
+}
+
+#endif /* !__MATH_PLANE_H__ */

neo/idlib/math/Pluecker.cpp

@@ -0,0 +1,86 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+					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			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() const;										// pluecker length
+	float			LengthSqr() const;									// pluecker squared length
+	idPluecker		Normalize() const;									// pluecker normalize
+	float			NormalizeSelf();										// pluecker normalize
+
+	int				GetDimension() const;
+
+	const float *	ToFloatPtr() const;
+	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+
+private:
+	float			p[6];
+};
+
+extern idPluecker pluecker_origin;
+#define pluecker_zero pluecker_origin
+
+ID_INLINE idPluecker::idPluecker() {
+}
+
+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() {
+	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() const {
+	return ( float )idMath::Sqrt( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
+}
+
+ID_INLINE float idPluecker::LengthSqr() const {
+	return ( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
+}
+
+ID_INLINE float idPluecker::NormalizeSelf() {
+	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() 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() const {
+	return 6;
+}
+
+ID_INLINE const float *idPluecker::ToFloatPtr() const {
+	return p;
+}
+
+ID_INLINE float *idPluecker::ToFloatPtr() {
+	return p;
+}
+
+#endif /* !__MATH_PLUECKER_H__ */

neo/idlib/math/Polynomial.cpp

@@ -0,0 +1,242 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+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() {
+	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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+					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			Zero( int d );
+
+	int				GetDimension() const;									// get the degree of the polynomial
+	int				GetDegree() 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() const;								// get the first derivative of the polynomial
+	idPolynomial	GetAntiDerivative() 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() const;
+	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+
+	static void		Test();
+
+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() {
+	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() {
+	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() const {
+	return degree;
+}
+
+ID_INLINE int idPolynomial::GetDegree() 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() 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() 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() const {
+	return coefficient;
+}
+
+ID_INLINE float *idPolynomial::ToFloatPtr() {
+	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 ), TAG_MATH );
+		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,307 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+/*
+=====================
+idQuat::ToAngles
+=====================
+*/
+idAngles idQuat::ToAngles() const {
+	return ToMat3().ToAngles();
+}
+
+/*
+=====================
+idQuat::ToRotation
+=====================
+*/
+idRotation idQuat::ToRotation() 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() 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() const {
+	return ToMat3().ToMat4();
+}
+
+/*
+=====================
+idQuat::ToCQuat
+=====================
+*/
+idCQuat idQuat::ToCQuat() const {
+	if ( w < 0.0f ) {
+		return idCQuat( -x, -y, -z );
+	}
+	return idCQuat( x, y, z );
+}
+
+/*
+============
+idQuat::ToAngularVelocity
+============
+*/
+idVec3 idQuat::ToAngularVelocity() 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;
+}
+
+/*
+========================
+idQuat::Lerp
+
+Approximation of spherical linear interpolation between two quaternions. The interpolation 
+traces out the exact same curve as Slerp but does not maintain a constant speed across the arc.
+========================
+*/
+idQuat &idQuat::Lerp( const idQuat &from, const idQuat &to, const float t ) {
+	if ( t <= 0.0f ) {
+		*this = from;
+		return *this;
+	}
+
+	if ( t >= 1.0f ) {
+		*this = to;
+		return *this;
+	}
+
+	if ( from == to ) {
+		*this = to;
+		return *this;
+	}
+
+	float cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
+
+	float scale0 = 1.0f - t;
+	float scale1 = ( cosom >= 0.0f ) ? t : -t;
+
+	x = scale0 * from.x + scale1 * to.x;
+	y = scale0 * from.y + scale1 * to.y;
+	z = scale0 * from.z + scale1 * to.z;
+	w = scale0 * from.w + scale1 * to.w;
+
+	float s = idMath::InvSqrt( x * x + y * y + z * z + w * w );
+
+	x *= s;
+	y *= s;
+	z *= s;
+	w *= s;
+
+	return *this;
+}
+
+/*
+=============
+idCQuat::ToAngles
+=============
+*/
+idAngles idCQuat::ToAngles() const {
+	return ToQuat().ToAngles();
+}
+
+/*
+=============
+idCQuat::ToRotation
+=============
+*/
+idRotation idCQuat::ToRotation() const {
+	return ToQuat().ToRotation();
+}
+
+/*
+=============
+idCQuat::ToMat3
+=============
+*/
+idMat3 idCQuat::ToMat3() const {
+	return ToQuat().ToMat3();
+}
+
+/*
+=============
+idCQuat::ToMat4
+=============
+*/
+idMat4 idCQuat::ToMat4() const {
+	return ToQuat().ToMat4();
+}
+
+/*
+=============
+idCQuat::ToString
+=============
+*/
+const char *idCQuat::ToString( int precision ) const {
+	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
+}
+
+/*
+=====================
+Slerp
+
+Spherical linear interpolation between two quaternions.
+=====================
+*/
+idQuat Slerp( const idQuat & from, const idQuat & to, const float t ) {
+	return idQuat().Slerp( from, to, t );
+}

neo/idlib/math/Quat.h

@@ -0,0 +1,433 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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
+
+===============================================================================
+*/
+
+#include "../containers/Array.h"	// for idTupleSize
+
+class idVec3;
+class idAngles;
+class idRotation;
+class idMat3;
+class idMat4;
+class idCQuat;
+
+class idQuat {
+public:
+	float			x;
+	float			y;
+	float			z;
+	float			w;
+
+					idQuat();
+					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() const;
+	float			Length() const;
+	idQuat &		Normalize();
+
+	float			CalcW() const;
+	int				GetDimension() const;
+
+	idAngles		ToAngles() const;
+	idRotation		ToRotation() const;
+	idMat3			ToMat3() const;
+	idMat4			ToMat4() const;
+	idCQuat			ToCQuat() const;
+	idVec3			ToAngularVelocity() const;
+	const float *	ToFloatPtr() const;
+	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+
+	idQuat &		Slerp( const idQuat &from, const idQuat &to, float t );
+	idQuat &		Lerp( const idQuat &from, const idQuat &to, const float t );
+};
+
+// A non-member slerp function allows constructing a const idQuat object with the result of a slerp,
+// but without having to explicity create a temporary idQuat object.
+idQuat Slerp( const idQuat & from, const idQuat & to, const float t );
+
+ID_INLINE idQuat::idQuat() {
+}
+
+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() const {
+	return idQuat( -x, -y, -z, w );
+}
+
+ID_INLINE float idQuat::Length() const {
+	float len;
+
+	len = x * x + y * y + z * z + w * w;
+	return idMath::Sqrt( len );
+}
+
+ID_INLINE idQuat& idQuat::Normalize() {
+	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() 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() const {
+	return 4;
+}
+
+ID_INLINE const float *idQuat::ToFloatPtr() const {
+	return &x;
+}
+
+ID_INLINE float *idQuat::ToFloatPtr() {
+	return &x;
+}
+
+/*
+===============================================================================
+
+	Specialization to get size of an idQuat generically.
+
+===============================================================================
+*/
+template<>
+struct idTupleSize< idQuat > {
+	enum { value = 4 };
+};
+
+/*
+===============================================================================
+
+	Compressed quaternion
+
+===============================================================================
+*/
+
+class idCQuat {
+public:
+	float			x;
+	float			y;
+	float			z;
+
+					idCQuat();
+					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() const;
+
+	idAngles		ToAngles() const;
+	idRotation		ToRotation() const;
+	idMat3			ToMat3() const;
+	idMat4			ToMat4() const;
+	idQuat			ToQuat() const;
+	const float *	ToFloatPtr() const;
+	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+};
+
+ID_INLINE idCQuat::idCQuat() {
+}
+
+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() const {
+	return 3;
+}
+
+ID_INLINE idQuat idCQuat::ToQuat() 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() const {
+	return &x;
+}
+
+ID_INLINE float *idCQuat::ToFloatPtr() {
+	return &x;
+}
+
+/*
+===============================================================================
+
+	Specialization to get size of an idCQuat generically.
+
+===============================================================================
+*/
+template<>
+struct idTupleSize< idCQuat > {
+	enum { value = 3 };
+};
+
+#endif /* !__MATH_QUAT_H__ */

neo/idlib/math/Random.h

@@ -0,0 +1,158 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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() const;
+
+	int					RandomInt();			// random integer in the range [0, MAX_RAND]
+	int					RandomInt( int max );		// random integer in the range [0, max[
+	float				RandomFloat();		// random number in the range [0.0f, 1.0f]
+	float				CRandomFloat();		// 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() const {
+	return seed;
+}
+
+ID_INLINE int idRandom::RandomInt() {
+	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() {
+	return ( RandomInt() / ( float )( idRandom::MAX_RAND + 1 ) );
+}
+
+ID_INLINE float idRandom::CRandomFloat() {
+	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() const;
+
+	int						RandomInt();			// random integer in the range [0, MAX_RAND]
+	int						RandomInt( int max );		// random integer in the range [0, max]
+	float					RandomFloat();		// random number in the range [0.0f, 1.0f]
+	float					CRandomFloat();		// 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() const {
+	return seed;
+}
+
+ID_INLINE int idRandom2::RandomInt() {
+	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() {
+	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() {
+	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,156 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+/*
+============
+idRotation::ToAngles
+============
+*/
+idAngles idRotation::ToAngles() const {
+	return ToMat3().ToAngles();
+}
+
+/*
+============
+idRotation::ToQuat
+============
+*/
+idQuat idRotation::ToQuat() 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() 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() const {
+	return ToMat3().ToMat4();
+}
+
+/*
+============
+idRotation::ToAngularVelocity
+============
+*/
+idVec3 idRotation::ToAngularVelocity() const {
+	return vec * DEG2RAD( angle );
+}
+
+/*
+============
+idRotation::Normalize180
+============
+*/
+void idRotation::Normalize180() {
+	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() {
+	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 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+						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();
+	const idVec3 &		GetOrigin() const;
+	const idVec3 &		GetVec() const;
+	float				GetAngle() 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() const;
+	idQuat				ToQuat() const;
+	const idMat3 &		ToMat3() const;
+	idMat4				ToMat4() const;
+	idVec3				ToAngularVelocity() const;
+
+	void				RotatePoint( idVec3 &point ) const;
+
+	void				Normalize180();
+	void				Normalize360();
+
+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() {
+}
+
+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() {
+	axisValid = false;
+	ToMat3();
+}
+
+ID_INLINE const idVec3 &idRotation::GetOrigin() const {
+	return origin;
+}
+
+ID_INLINE const idVec3 &idRotation::GetVec() const  {
+	return vec;
+}
+
+ID_INLINE float idRotation::GetAngle() 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,109 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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();
+	static void			InitProcessor( const char *module, bool forceGeneric );
+	static void			Shutdown();
+	static void			Test_f( const class idCmdArgs &args );
+};
+
+
+/*
+===============================================================================
+
+	virtual base class for different SIMD processors
+
+===============================================================================
+*/
+
+#define VPCALL __fastcall
+
+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_t;
+
+class idSIMDProcessor {
+public:
+									idSIMDProcessor() { cpuid = CPUID_NONE; }
+
+	cpuid_t							cpuid;
+
+	virtual const char * VPCALL		GetName() const = 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 triIndex_t *indexes,		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;
+
+	// animation
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) = 0;
+	virtual void VPCALL BlendJointsFast( 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;
+};
+
+// pointer to SIMD processor
+extern idSIMDProcessor *SIMDProcessor;
+
+#endif /* !__MATH_SIMD_H__ */

neo/idlib/math/Simd_Generic.cpp

@@ -0,0 +1,211 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+#include "Simd_Generic.h"
+
+//===============================================================
+//
+//	Generic implementation of idSIMDProcessor
+//
+//===============================================================
+
+#define UNROLL1(Y) { int _IX; for (_IX=0;_IX<count;_IX++) {Y(_IX);} }
+#define UNROLL2(Y) { int _IX, _NM = count&0xfffffffe; for (_IX=0;_IX<_NM;_IX+=2){Y(_IX+0);Y(_IX+1);} if (_IX < count) {Y(_IX);}}
+#define UNROLL4(Y) { int _IX, _NM = count&0xfffffffc; for (_IX=0;_IX<_NM;_IX+=4){Y(_IX+0);Y(_IX+1);Y(_IX+2);Y(_IX+3);}for(;_IX<count;_IX++){Y(_IX);}}
+#define UNROLL8(Y) { int _IX, _NM = count&0xfffffff8; for (_IX=0;_IX<_NM;_IX+=8){Y(_IX+0);Y(_IX+1);Y(_IX+2);Y(_IX+3);Y(_IX+4);Y(_IX+5);Y(_IX+6);Y(_IX+7);} _NM = count&0xfffffffe; for(;_IX<_NM;_IX+=2){Y(_IX); Y(_IX+1);} if (_IX < count) {Y(_IX);} }
+
+#ifdef _DEBUG
+#define NODEFAULT	default: assert( 0 )
+#else
+#define NODEFAULT	default: __assume( 0 )
+#endif
+
+
+/*
+============
+idSIMD_Generic::GetName
+============
+*/
+const char * idSIMD_Generic::GetName() const {
+	return "generic code";
+}
+
+/*
+============
+idSIMD_Generic::MinMax
+============
+*/
+void VPCALL idSIMD_Generic::MinMax( float &min, float &max, const float *src, const int count ) {
+	min = idMath::INFINITY; max = -idMath::INFINITY;
+#define OPER(X) if ( src[(X)] < min ) {min = src[(X)];} if ( src[(X)] > max ) {max = src[(X)];}
+	UNROLL1(OPER)
+#undef OPER
+}
+
+/*
+============
+idSIMD_Generic::MinMax
+============
+*/
+void VPCALL idSIMD_Generic::MinMax( idVec2 &min, idVec2 &max, const idVec2 *src, const int count ) {
+	min[0] = min[1] = idMath::INFINITY; max[0] = max[1] = -idMath::INFINITY;
+#define OPER(X) const idVec2 &v = src[(X)]; if ( v[0] < min[0] ) { min[0] = v[0]; } if ( v[0] > max[0] ) { max[0] = v[0]; } if ( v[1] < min[1] ) { min[1] = v[1]; } if ( v[1] > max[1] ) { max[1] = v[1]; }
+	UNROLL1(OPER)
+#undef OPER
+}
+
+/*
+============
+idSIMD_Generic::MinMax
+============
+*/
+void VPCALL idSIMD_Generic::MinMax( idVec3 &min, idVec3 &max, const idVec3 *src, const int count ) {
+	min[0] = min[1] = min[2] = idMath::INFINITY; max[0] = max[1] = max[2] = -idMath::INFINITY;
+#define OPER(X) const idVec3 &v = src[(X)]; if ( v[0] < min[0] ) { min[0] = v[0]; } if ( v[0] > max[0] ) { max[0] = v[0]; } if ( v[1] < min[1] ) { min[1] = v[1]; } if ( v[1] > max[1] ) { max[1] = v[1]; } if ( v[2] < min[2] ) { min[2] = v[2]; } if ( v[2] > max[2] ) { max[2] = v[2]; }
+	UNROLL1(OPER)
+#undef OPER
+}
+
+/*
+============
+idSIMD_Generic::MinMax
+============
+*/
+void VPCALL idSIMD_Generic::MinMax( idVec3 &min, idVec3 &max, const idDrawVert *src, const int count ) {
+	min[0] = min[1] = min[2] = idMath::INFINITY; max[0] = max[1] = max[2] = -idMath::INFINITY;
+#define OPER(X) const idVec3 &v = src[(X)].xyz; if ( v[0] < min[0] ) { min[0] = v[0]; } if ( v[0] > max[0] ) { max[0] = v[0]; } if ( v[1] < min[1] ) { min[1] = v[1]; } if ( v[1] > max[1] ) { max[1] = v[1]; } if ( v[2] < min[2] ) { min[2] = v[2]; } if ( v[2] > max[2] ) { max[2] = v[2]; }
+	UNROLL1(OPER)
+#undef OPER
+}
+
+/*
+============
+idSIMD_Generic::MinMax
+============
+*/
+void VPCALL idSIMD_Generic::MinMax( idVec3 &min, idVec3 &max, const idDrawVert *src, const triIndex_t *indexes, const int count ) {
+	min[0] = min[1] = min[2] = idMath::INFINITY; max[0] = max[1] = max[2] = -idMath::INFINITY;
+#define OPER(X) const idVec3 &v = src[indexes[(X)]].xyz; if ( v[0] < min[0] ) { min[0] = v[0]; } if ( v[0] > max[0] ) { max[0] = v[0]; } if ( v[1] < min[1] ) { min[1] = v[1]; } if ( v[1] > max[1] ) { max[1] = v[1]; } if ( v[2] < min[2] ) { min[2] = v[2]; } if ( v[2] > max[2] ) { max[2] = v[2]; }
+	UNROLL1(OPER)
+#undef OPER
+}
+
+/*
+================
+idSIMD_Generic::Memcpy
+================
+*/
+void VPCALL idSIMD_Generic::Memcpy( void *dst, const void *src, const int count ) {
+	memcpy( dst, src, count );
+}
+
+/*
+================
+idSIMD_Generic::Memset
+================
+*/
+void VPCALL idSIMD_Generic::Memset( void *dst, const int val, const int count ) {
+	memset( dst, val, count );
+}
+
+/*
+============
+idSIMD_Generic::BlendJoints
+============
+*/
+void VPCALL idSIMD_Generic::BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
+	for ( int i = 0; i < numJoints; i++ ) {
+		int j = index[i];
+		joints[j].q.Slerp( joints[j].q, blendJoints[j].q, lerp );
+		joints[j].t.Lerp( joints[j].t, blendJoints[j].t, lerp );
+		joints[j].w = 0.0f;
+	}
+}
+
+/*
+============
+idSIMD_Generic::BlendJointsFast
+============
+*/
+void VPCALL idSIMD_Generic::BlendJointsFast( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
+	for ( int i = 0; i < numJoints; i++ ) {
+		int j = index[i];
+		joints[j].q.Lerp( joints[j].q, blendJoints[j].q, lerp );
+		joints[j].t.Lerp( joints[j].t, blendJoints[j].t, lerp );
+		joints[j].w = 0.0f;
+	}
+}
+
+/*
+============
+idSIMD_Generic::ConvertJointQuatsToJointMats
+============
+*/
+void VPCALL idSIMD_Generic::ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints ) {
+	for ( int i = 0; i < numJoints; i++ ) {
+		jointMats[i].SetRotation( jointQuats[i].q.ToMat3() );
+		jointMats[i].SetTranslation( jointQuats[i].t );
+	}
+}
+
+/*
+============
+idSIMD_Generic::ConvertJointMatsToJointQuats
+============
+*/
+void VPCALL idSIMD_Generic::ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints ) {
+	for ( int i = 0; i < numJoints; i++ ) {
+		jointQuats[i] = jointMats[i].ToJointQuat();
+	}
+}
+
+/*
+============
+idSIMD_Generic::TransformJoints
+============
+*/
+void VPCALL idSIMD_Generic::TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
+	for ( int i = firstJoint; i <= lastJoint; i++ ) {
+		assert( parents[i] < i );
+		jointMats[i] *= jointMats[parents[i]];
+	}
+}
+
+/*
+============
+idSIMD_Generic::UntransformJoints
+============
+*/
+void VPCALL idSIMD_Generic::UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
+	for ( int i = lastJoint; i >= firstJoint; i-- ) {
+		assert( parents[i] < i );
+		jointMats[i] /= jointMats[parents[i]];
+	}
+}

neo/idlib/math/Simd_Generic.h

@@ -0,0 +1,61 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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() const;
+
+	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 triIndex_t *indexes,		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 BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints );
+	virtual void VPCALL BlendJointsFast( 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 );
+};
+
+#endif /* !__MATH_SIMD_GENERIC_H__ */

neo/idlib/math/Simd_SSE.cpp

@@ -0,0 +1,934 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+#include "Simd_Generic.h"
+#include "Simd_SSE.h"
+
+//===============================================================
+//                                                        M
+//  SSE implementation of idSIMDProcessor                MrE
+//                                                        E
+//===============================================================
+
+
+#include <xmmintrin.h>
+
+#define M_PI	3.14159265358979323846f
+
+/*
+============
+idSIMD_SSE::GetName
+============
+*/
+const char * idSIMD_SSE::GetName() const {
+	return "MMX & SSE";
+}
+
+/*
+============
+idSIMD_SSE::BlendJoints
+============
+*/
+void VPCALL idSIMD_SSE::BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
+
+	if ( lerp <= 0.0f ) {
+		return;
+	} else if ( lerp >= 1.0f ) {
+		for ( int i = 0; i < numJoints; i++ ) {
+			int j = index[i];
+			joints[j] = blendJoints[j];
+		}
+		return;
+	}
+
+	const __m128 vlerp = { lerp, lerp, lerp, lerp };
+
+	const __m128 vector_float_one		= { 1.0f, 1.0f, 1.0f, 1.0f };
+	const __m128 vector_float_sign_bit	= __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
+	const __m128 vector_float_rsqrt_c0	= {  -3.0f,  -3.0f,  -3.0f,  -3.0f };
+	const __m128 vector_float_rsqrt_c1	= {  -0.5f,  -0.5f,  -0.5f,  -0.5f };
+	const __m128 vector_float_tiny		= {    1e-10f,    1e-10f,    1e-10f,    1e-10f };
+	const __m128 vector_float_half_pi	= { M_PI*0.5f, M_PI*0.5f, M_PI*0.5f, M_PI*0.5f };
+
+	const __m128 vector_float_sin_c0	= { -2.39e-08f, -2.39e-08f, -2.39e-08f, -2.39e-08f };
+	const __m128 vector_float_sin_c1	= {  2.7526e-06f, 2.7526e-06f, 2.7526e-06f, 2.7526e-06f };
+	const __m128 vector_float_sin_c2	= { -1.98409e-04f, -1.98409e-04f, -1.98409e-04f, -1.98409e-04f };
+	const __m128 vector_float_sin_c3	= {  8.3333315e-03f, 8.3333315e-03f, 8.3333315e-03f, 8.3333315e-03f };
+	const __m128 vector_float_sin_c4	= { -1.666666664e-01f, -1.666666664e-01f, -1.666666664e-01f, -1.666666664e-01f };
+
+	const __m128 vector_float_atan_c0	= {  0.0028662257f,  0.0028662257f,  0.0028662257f,  0.0028662257f };
+	const __m128 vector_float_atan_c1	= { -0.0161657367f, -0.0161657367f, -0.0161657367f, -0.0161657367f };
+	const __m128 vector_float_atan_c2	= {  0.0429096138f,  0.0429096138f,  0.0429096138f,  0.0429096138f };
+	const __m128 vector_float_atan_c3	= { -0.0752896400f, -0.0752896400f, -0.0752896400f, -0.0752896400f };
+	const __m128 vector_float_atan_c4	= {  0.1065626393f,  0.1065626393f,  0.1065626393f,  0.1065626393f };
+	const __m128 vector_float_atan_c5	= { -0.1420889944f, -0.1420889944f, -0.1420889944f, -0.1420889944f };
+	const __m128 vector_float_atan_c6	= {  0.1999355085f,  0.1999355085f,  0.1999355085f,  0.1999355085f };
+	const __m128 vector_float_atan_c7	= { -0.3333314528f, -0.3333314528f, -0.3333314528f, -0.3333314528f };
+
+	int i = 0;
+	for ( ; i < numJoints - 3; i += 4 ) {
+		const int n0 = index[i+0];
+		const int n1 = index[i+1];
+		const int n2 = index[i+2];
+		const int n3 = index[i+3];
+
+		__m128 jqa_0 = _mm_load_ps( joints[n0].q.ToFloatPtr() );
+		__m128 jqb_0 = _mm_load_ps( joints[n1].q.ToFloatPtr() );
+		__m128 jqc_0 = _mm_load_ps( joints[n2].q.ToFloatPtr() );
+		__m128 jqd_0 = _mm_load_ps( joints[n3].q.ToFloatPtr() );
+
+		__m128 jta_0 = _mm_load_ps( joints[n0].t.ToFloatPtr() );
+		__m128 jtb_0 = _mm_load_ps( joints[n1].t.ToFloatPtr() );
+		__m128 jtc_0 = _mm_load_ps( joints[n2].t.ToFloatPtr() );
+		__m128 jtd_0 = _mm_load_ps( joints[n3].t.ToFloatPtr() );
+
+		__m128 bqa_0 = _mm_load_ps( blendJoints[n0].q.ToFloatPtr() );
+		__m128 bqb_0 = _mm_load_ps( blendJoints[n1].q.ToFloatPtr() );
+		__m128 bqc_0 = _mm_load_ps( blendJoints[n2].q.ToFloatPtr() );
+		__m128 bqd_0 = _mm_load_ps( blendJoints[n3].q.ToFloatPtr() );
+
+		__m128 bta_0 = _mm_load_ps( blendJoints[n0].t.ToFloatPtr() );
+		__m128 btb_0 = _mm_load_ps( blendJoints[n1].t.ToFloatPtr() );
+		__m128 btc_0 = _mm_load_ps( blendJoints[n2].t.ToFloatPtr() );
+		__m128 btd_0 = _mm_load_ps( blendJoints[n3].t.ToFloatPtr() );
+
+		bta_0 = _mm_sub_ps( bta_0, jta_0 );
+		btb_0 = _mm_sub_ps( btb_0, jtb_0 );
+		btc_0 = _mm_sub_ps( btc_0, jtc_0 );
+		btd_0 = _mm_sub_ps( btd_0, jtd_0 );
+
+		jta_0 = _mm_madd_ps( vlerp, bta_0, jta_0 );
+		jtb_0 = _mm_madd_ps( vlerp, btb_0, jtb_0 );
+		jtc_0 = _mm_madd_ps( vlerp, btc_0, jtc_0 );
+		jtd_0 = _mm_madd_ps( vlerp, btd_0, jtd_0 );
+
+		_mm_store_ps( joints[n0].t.ToFloatPtr(), jta_0 );
+		_mm_store_ps( joints[n1].t.ToFloatPtr(), jtb_0 );
+		_mm_store_ps( joints[n2].t.ToFloatPtr(), jtc_0 );
+		_mm_store_ps( joints[n3].t.ToFloatPtr(), jtd_0 );
+
+		__m128 jqr_0 = _mm_unpacklo_ps( jqa_0, jqc_0 );
+		__m128 jqs_0 = _mm_unpackhi_ps( jqa_0, jqc_0 );
+		__m128 jqt_0 = _mm_unpacklo_ps( jqb_0, jqd_0 );
+		__m128 jqu_0 = _mm_unpackhi_ps( jqb_0, jqd_0 );
+
+		__m128 bqr_0 = _mm_unpacklo_ps( bqa_0, bqc_0 );
+		__m128 bqs_0 = _mm_unpackhi_ps( bqa_0, bqc_0 );
+		__m128 bqt_0 = _mm_unpacklo_ps( bqb_0, bqd_0 );
+		__m128 bqu_0 = _mm_unpackhi_ps( bqb_0, bqd_0 );
+
+		__m128 jqx_0 = _mm_unpacklo_ps( jqr_0, jqt_0 );
+		__m128 jqy_0 = _mm_unpackhi_ps( jqr_0, jqt_0 );
+		__m128 jqz_0 = _mm_unpacklo_ps( jqs_0, jqu_0 );
+		__m128 jqw_0 = _mm_unpackhi_ps( jqs_0, jqu_0 );
+
+		__m128 bqx_0 = _mm_unpacklo_ps( bqr_0, bqt_0 );
+		__m128 bqy_0 = _mm_unpackhi_ps( bqr_0, bqt_0 );
+		__m128 bqz_0 = _mm_unpacklo_ps( bqs_0, bqu_0 );
+		__m128 bqw_0 = _mm_unpackhi_ps( bqs_0, bqu_0 );
+
+		__m128 cosoma_0 = _mm_mul_ps( jqx_0, bqx_0 );
+		__m128 cosomb_0 = _mm_mul_ps( jqy_0, bqy_0 );
+		__m128 cosomc_0 = _mm_mul_ps( jqz_0, bqz_0 );
+		__m128 cosomd_0 = _mm_mul_ps( jqw_0, bqw_0 );
+
+		__m128 cosome_0 = _mm_add_ps( cosoma_0, cosomb_0 );
+		__m128 cosomf_0 = _mm_add_ps( cosomc_0, cosomd_0 );
+		__m128 cosomg_0 = _mm_add_ps( cosome_0, cosomf_0 );
+
+		__m128 sign_0 = _mm_and_ps( cosomg_0, vector_float_sign_bit );
+		__m128 cosom_0 = _mm_xor_ps( cosomg_0, sign_0 );
+		__m128 ss_0 = _mm_nmsub_ps( cosom_0, cosom_0, vector_float_one );
+
+		ss_0 = _mm_max_ps( ss_0, vector_float_tiny );
+
+		__m128 rs_0 = _mm_rsqrt_ps( ss_0 );
+		__m128 sq_0 = _mm_mul_ps( rs_0, rs_0 );
+		__m128 sh_0 = _mm_mul_ps( rs_0, vector_float_rsqrt_c1 );
+		__m128 sx_0 = _mm_madd_ps( ss_0, sq_0, vector_float_rsqrt_c0 );
+		__m128 sinom_0 = _mm_mul_ps( sh_0, sx_0 );						// sinom = sqrt( ss );
+
+		ss_0 = _mm_mul_ps( ss_0, sinom_0 );
+
+		__m128 min_0 = _mm_min_ps( ss_0, cosom_0 );
+		__m128 max_0 = _mm_max_ps( ss_0, cosom_0 );
+		__m128 mask_0 = _mm_cmpeq_ps( min_0, cosom_0 );
+		__m128 masksign_0 = _mm_and_ps( mask_0, vector_float_sign_bit );
+		__m128 maskPI_0 = _mm_and_ps( mask_0, vector_float_half_pi );
+
+		__m128 rcpa_0 = _mm_rcp_ps( max_0 );
+		__m128 rcpb_0 = _mm_mul_ps( max_0, rcpa_0 );
+		__m128 rcpd_0 = _mm_add_ps( rcpa_0, rcpa_0 );
+		__m128 rcp_0 = _mm_nmsub_ps( rcpb_0, rcpa_0, rcpd_0 );			// 1 / y or 1 / x
+		__m128 ata_0 = _mm_mul_ps( min_0, rcp_0 );						// x / y or y / x
+
+		__m128 atb_0 = _mm_xor_ps( ata_0, masksign_0 );					// -x / y or y / x
+		__m128 atc_0 = _mm_mul_ps( atb_0, atb_0 );
+		__m128 atd_0 = _mm_madd_ps( atc_0, vector_float_atan_c0, vector_float_atan_c1 );
+
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c2 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c3 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c4 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c5 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c6 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c7 );
+		atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_one );
+
+		__m128 omega_a_0 = _mm_madd_ps( atd_0, atb_0, maskPI_0 );
+		__m128 omega_b_0 = _mm_mul_ps( vlerp, omega_a_0 );
+		omega_a_0 = _mm_sub_ps( omega_a_0, omega_b_0 );
+
+		__m128 sinsa_0 = _mm_mul_ps( omega_a_0, omega_a_0 );
+		__m128 sinsb_0 = _mm_mul_ps( omega_b_0, omega_b_0 );
+		__m128 sina_0 = _mm_madd_ps( sinsa_0, vector_float_sin_c0, vector_float_sin_c1 );
+		__m128 sinb_0 = _mm_madd_ps( sinsb_0, vector_float_sin_c0, vector_float_sin_c1 );
+		sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c2 );
+		sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c2 );
+		sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c3 );
+		sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c3 );
+		sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c4 );
+		sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c4 );
+		sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_one );
+		sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_one );
+		sina_0 = _mm_mul_ps( sina_0, omega_a_0 );
+		sinb_0 = _mm_mul_ps( sinb_0, omega_b_0 );
+		__m128 scalea_0 = _mm_mul_ps( sina_0, sinom_0 );
+		__m128 scaleb_0 = _mm_mul_ps( sinb_0, sinom_0 );
+
+		scaleb_0 = _mm_xor_ps( scaleb_0, sign_0 );
+
+		jqx_0 = _mm_mul_ps( jqx_0, scalea_0 );
+		jqy_0 = _mm_mul_ps( jqy_0, scalea_0 );
+		jqz_0 = _mm_mul_ps( jqz_0, scalea_0 );
+		jqw_0 = _mm_mul_ps( jqw_0, scalea_0 );
+
+		jqx_0 = _mm_madd_ps( bqx_0, scaleb_0, jqx_0 );
+		jqy_0 = _mm_madd_ps( bqy_0, scaleb_0, jqy_0 );
+		jqz_0 = _mm_madd_ps( bqz_0, scaleb_0, jqz_0 );
+		jqw_0 = _mm_madd_ps( bqw_0, scaleb_0, jqw_0 );
+
+		__m128 tp0_0 = _mm_unpacklo_ps( jqx_0, jqz_0 );
+		__m128 tp1_0 = _mm_unpackhi_ps( jqx_0, jqz_0 );
+		__m128 tp2_0 = _mm_unpacklo_ps( jqy_0, jqw_0 );
+		__m128 tp3_0 = _mm_unpackhi_ps( jqy_0, jqw_0 );
+
+		__m128 p0_0 = _mm_unpacklo_ps( tp0_0, tp2_0 );
+		__m128 p1_0 = _mm_unpackhi_ps( tp0_0, tp2_0 );
+		__m128 p2_0 = _mm_unpacklo_ps( tp1_0, tp3_0 );
+		__m128 p3_0 = _mm_unpackhi_ps( tp1_0, tp3_0 );
+
+		_mm_store_ps( joints[n0].q.ToFloatPtr(), p0_0 );
+		_mm_store_ps( joints[n1].q.ToFloatPtr(), p1_0 );
+		_mm_store_ps( joints[n2].q.ToFloatPtr(), p2_0 );
+		_mm_store_ps( joints[n3].q.ToFloatPtr(), p3_0 );
+	}
+
+	for ( ; i < numJoints; i++ ) {
+		int n = index[i];
+
+		idVec3 &jointVert = joints[n].t;
+		const idVec3 &blendVert = blendJoints[n].t;
+
+		jointVert[0] += lerp * ( blendVert[0] - jointVert[0] );
+		jointVert[1] += lerp * ( blendVert[1] - jointVert[1] );
+		jointVert[2] += lerp * ( blendVert[2] - jointVert[2] );
+		joints[n].w = 0.0f;
+
+		idQuat &jointQuat = joints[n].q;
+		const idQuat &blendQuat = blendJoints[n].q;
+
+		float cosom;
+		float sinom;
+		float omega;
+		float scale0;
+		float scale1;
+		unsigned long signBit;
+
+		cosom = jointQuat.x * blendQuat.x + jointQuat.y * blendQuat.y + jointQuat.z * blendQuat.z + jointQuat.w * blendQuat.w;
+
+		signBit = (*(unsigned long *)&cosom) & ( 1 << 31 );
+
+		(*(unsigned long *)&cosom) ^= signBit;
+
+		scale0 = 1.0f - cosom * cosom;
+		scale0 = ( scale0 <= 0.0f ) ? 1e-10f : scale0;
+		sinom = idMath::InvSqrt( scale0 );
+		omega = idMath::ATan16( scale0 * sinom, cosom );
+		scale0 = idMath::Sin16( ( 1.0f - lerp ) * omega ) * sinom;
+		scale1 = idMath::Sin16( lerp * omega ) * sinom;
+
+		(*(unsigned long *)&scale1) ^= signBit;
+
+		jointQuat.x = scale0 * jointQuat.x + scale1 * blendQuat.x;
+		jointQuat.y = scale0 * jointQuat.y + scale1 * blendQuat.y;
+		jointQuat.z = scale0 * jointQuat.z + scale1 * blendQuat.z;
+		jointQuat.w = scale0 * jointQuat.w + scale1 * blendQuat.w;
+	}
+}
+
+/*
+============
+idSIMD_SSE::BlendJointsFast
+============
+*/
+void VPCALL idSIMD_SSE::BlendJointsFast( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
+	assert_16_byte_aligned( joints );
+	assert_16_byte_aligned( blendJoints );
+	assert_16_byte_aligned( JOINTQUAT_Q_OFFSET );
+	assert_16_byte_aligned( JOINTQUAT_T_OFFSET );
+	assert_sizeof_16_byte_multiple( idJointQuat );
+
+	if ( lerp <= 0.0f ) {
+		return;
+	} else if ( lerp >= 1.0f ) {
+		for ( int i = 0; i < numJoints; i++ ) {
+			int j = index[i];
+			joints[j] = blendJoints[j];
+		}
+		return;
+	}
+
+	const __m128 vector_float_sign_bit	= __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
+	const __m128 vector_float_rsqrt_c0	= {  -3.0f,  -3.0f,  -3.0f,  -3.0f };
+	const __m128 vector_float_rsqrt_c1	= {  -0.5f,  -0.5f,  -0.5f,  -0.5f };
+
+	const float scaledLerp = lerp / ( 1.0f - lerp );
+	const __m128 vlerp = { lerp, lerp, lerp, lerp };
+	const __m128 vscaledLerp = { scaledLerp, scaledLerp, scaledLerp, scaledLerp };
+
+	int i = 0;
+	for ( ; i < numJoints - 3; i += 4 ) {
+		const int n0 = index[i+0];
+		const int n1 = index[i+1];
+		const int n2 = index[i+2];
+		const int n3 = index[i+3];
+
+		__m128 jqa_0 = _mm_load_ps( joints[n0].q.ToFloatPtr() );
+		__m128 jqb_0 = _mm_load_ps( joints[n1].q.ToFloatPtr() );
+		__m128 jqc_0 = _mm_load_ps( joints[n2].q.ToFloatPtr() );
+		__m128 jqd_0 = _mm_load_ps( joints[n3].q.ToFloatPtr() );
+
+		__m128 jta_0 = _mm_load_ps( joints[n0].t.ToFloatPtr() );
+		__m128 jtb_0 = _mm_load_ps( joints[n1].t.ToFloatPtr() );
+		__m128 jtc_0 = _mm_load_ps( joints[n2].t.ToFloatPtr() );
+		__m128 jtd_0 = _mm_load_ps( joints[n3].t.ToFloatPtr() );
+
+		__m128 bqa_0 = _mm_load_ps( blendJoints[n0].q.ToFloatPtr() );
+		__m128 bqb_0 = _mm_load_ps( blendJoints[n1].q.ToFloatPtr() );
+		__m128 bqc_0 = _mm_load_ps( blendJoints[n2].q.ToFloatPtr() );
+		__m128 bqd_0 = _mm_load_ps( blendJoints[n3].q.ToFloatPtr() );
+
+		__m128 bta_0 = _mm_load_ps( blendJoints[n0].t.ToFloatPtr() );
+		__m128 btb_0 = _mm_load_ps( blendJoints[n1].t.ToFloatPtr() );
+		__m128 btc_0 = _mm_load_ps( blendJoints[n2].t.ToFloatPtr() );
+		__m128 btd_0 = _mm_load_ps( blendJoints[n3].t.ToFloatPtr() );
+
+		bta_0 = _mm_sub_ps( bta_0, jta_0 );
+		btb_0 = _mm_sub_ps( btb_0, jtb_0 );
+		btc_0 = _mm_sub_ps( btc_0, jtc_0 );
+		btd_0 = _mm_sub_ps( btd_0, jtd_0 );
+
+		jta_0 = _mm_madd_ps( vlerp, bta_0, jta_0 );
+		jtb_0 = _mm_madd_ps( vlerp, btb_0, jtb_0 );
+		jtc_0 = _mm_madd_ps( vlerp, btc_0, jtc_0 );
+		jtd_0 = _mm_madd_ps( vlerp, btd_0, jtd_0 );
+
+		_mm_store_ps( joints[n0].t.ToFloatPtr(), jta_0 );
+		_mm_store_ps( joints[n1].t.ToFloatPtr(), jtb_0 );
+		_mm_store_ps( joints[n2].t.ToFloatPtr(), jtc_0 );
+		_mm_store_ps( joints[n3].t.ToFloatPtr(), jtd_0 );
+
+		__m128 jqr_0 = _mm_unpacklo_ps( jqa_0, jqc_0 );
+		__m128 jqs_0 = _mm_unpackhi_ps( jqa_0, jqc_0 );
+		__m128 jqt_0 = _mm_unpacklo_ps( jqb_0, jqd_0 );
+		__m128 jqu_0 = _mm_unpackhi_ps( jqb_0, jqd_0 );
+
+		__m128 bqr_0 = _mm_unpacklo_ps( bqa_0, bqc_0 );
+		__m128 bqs_0 = _mm_unpackhi_ps( bqa_0, bqc_0 );
+		__m128 bqt_0 = _mm_unpacklo_ps( bqb_0, bqd_0 );
+		__m128 bqu_0 = _mm_unpackhi_ps( bqb_0, bqd_0 );
+
+		__m128 jqx_0 = _mm_unpacklo_ps( jqr_0, jqt_0 );
+		__m128 jqy_0 = _mm_unpackhi_ps( jqr_0, jqt_0 );
+		__m128 jqz_0 = _mm_unpacklo_ps( jqs_0, jqu_0 );
+		__m128 jqw_0 = _mm_unpackhi_ps( jqs_0, jqu_0 );
+
+		__m128 bqx_0 = _mm_unpacklo_ps( bqr_0, bqt_0 );
+		__m128 bqy_0 = _mm_unpackhi_ps( bqr_0, bqt_0 );
+		__m128 bqz_0 = _mm_unpacklo_ps( bqs_0, bqu_0 );
+		__m128 bqw_0 = _mm_unpackhi_ps( bqs_0, bqu_0 );
+
+		__m128 cosoma_0 = _mm_mul_ps( jqx_0, bqx_0 );
+		__m128 cosomb_0 = _mm_mul_ps( jqy_0, bqy_0 );
+		__m128 cosomc_0 = _mm_mul_ps( jqz_0, bqz_0 );
+		__m128 cosomd_0 = _mm_mul_ps( jqw_0, bqw_0 );
+
+		__m128 cosome_0 = _mm_add_ps( cosoma_0, cosomb_0 );
+		__m128 cosomf_0 = _mm_add_ps( cosomc_0, cosomd_0 );
+		__m128 cosom_0 = _mm_add_ps( cosome_0, cosomf_0 );
+
+		__m128 sign_0 = _mm_and_ps( cosom_0, vector_float_sign_bit );
+
+		__m128 scale_0 = _mm_xor_ps( vscaledLerp, sign_0 );
+
+		jqx_0 = _mm_madd_ps( scale_0, bqx_0, jqx_0 );
+		jqy_0 = _mm_madd_ps( scale_0, bqy_0, jqy_0 );
+		jqz_0 = _mm_madd_ps( scale_0, bqz_0, jqz_0 );
+		jqw_0 = _mm_madd_ps( scale_0, bqw_0, jqw_0 );
+
+		__m128 da_0 = _mm_mul_ps( jqx_0, jqx_0 );
+		__m128 db_0 = _mm_mul_ps( jqy_0, jqy_0 );
+		__m128 dc_0 = _mm_mul_ps( jqz_0, jqz_0 );
+		__m128 dd_0 = _mm_mul_ps( jqw_0, jqw_0 );
+
+		__m128 de_0 = _mm_add_ps( da_0, db_0 );
+		__m128 df_0 = _mm_add_ps( dc_0, dd_0 );
+		__m128 d_0 = _mm_add_ps( de_0, df_0 );
+
+		__m128 rs_0 = _mm_rsqrt_ps( d_0 );
+		__m128 sq_0 = _mm_mul_ps( rs_0, rs_0 );
+		__m128 sh_0 = _mm_mul_ps( rs_0, vector_float_rsqrt_c1 );
+		__m128 sx_0 = _mm_madd_ps( d_0, sq_0, vector_float_rsqrt_c0 );
+		__m128 s_0 = _mm_mul_ps( sh_0, sx_0 );
+
+		jqx_0 = _mm_mul_ps( jqx_0, s_0 );
+		jqy_0 = _mm_mul_ps( jqy_0, s_0 );
+		jqz_0 = _mm_mul_ps( jqz_0, s_0 );
+		jqw_0 = _mm_mul_ps( jqw_0, s_0 );
+
+		__m128 tp0_0 = _mm_unpacklo_ps( jqx_0, jqz_0 );
+		__m128 tp1_0 = _mm_unpackhi_ps( jqx_0, jqz_0 );
+		__m128 tp2_0 = _mm_unpacklo_ps( jqy_0, jqw_0 );
+		__m128 tp3_0 = _mm_unpackhi_ps( jqy_0, jqw_0 );
+
+		__m128 p0_0 = _mm_unpacklo_ps( tp0_0, tp2_0 );
+		__m128 p1_0 = _mm_unpackhi_ps( tp0_0, tp2_0 );
+		__m128 p2_0 = _mm_unpacklo_ps( tp1_0, tp3_0 );
+		__m128 p3_0 = _mm_unpackhi_ps( tp1_0, tp3_0 );
+
+		_mm_store_ps( joints[n0].q.ToFloatPtr(), p0_0 );
+		_mm_store_ps( joints[n1].q.ToFloatPtr(), p1_0 );
+		_mm_store_ps( joints[n2].q.ToFloatPtr(), p2_0 );
+		_mm_store_ps( joints[n3].q.ToFloatPtr(), p3_0 );
+	}
+
+	for ( ; i < numJoints; i++ ) {
+		const int n = index[i];
+
+		idVec3 &jointVert = joints[n].t;
+		const idVec3 &blendVert = blendJoints[n].t;
+
+		jointVert[0] += lerp * ( blendVert[0] - jointVert[0] );
+		jointVert[1] += lerp * ( blendVert[1] - jointVert[1] );
+		jointVert[2] += lerp * ( blendVert[2] - jointVert[2] );
+
+		idQuat &jointQuat = joints[n].q;
+		const idQuat &blendQuat = blendJoints[n].q;
+
+		float cosom;
+		float scale;
+		float s;
+
+		cosom = jointQuat.x * blendQuat.x + jointQuat.y * blendQuat.y + jointQuat.z * blendQuat.z + jointQuat.w * blendQuat.w;
+
+		scale = __fsels( cosom, scaledLerp, -scaledLerp );
+
+		jointQuat.x += scale * blendQuat.x;
+		jointQuat.y += scale * blendQuat.y;
+		jointQuat.z += scale * blendQuat.z;
+		jointQuat.w += scale * blendQuat.w;
+
+		s = jointQuat.x * jointQuat.x + jointQuat.y * jointQuat.y + jointQuat.z * jointQuat.z + jointQuat.w * jointQuat.w;
+		s = __frsqrts( s );
+
+		jointQuat.x *= s;
+		jointQuat.y *= s;
+		jointQuat.z *= s;
+		jointQuat.w *= s;
+	}
+}
+
+/*
+============
+idSIMD_SSE::ConvertJointQuatsToJointMats
+============
+*/
+void VPCALL idSIMD_SSE::ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints ) {
+	assert( sizeof( idJointQuat ) == JOINTQUAT_SIZE );
+	assert( sizeof( idJointMat ) == JOINTMAT_SIZE );
+	assert( (int)(&((idJointQuat *)0)->t) == (int)(&((idJointQuat *)0)->q) + (int)sizeof( ((idJointQuat *)0)->q ) );
+
+	const float * jointQuatPtr = (float *)jointQuats;
+	float * jointMatPtr = (float *)jointMats;
+
+	const __m128 vector_float_first_sign_bit		= __m128c( _mm_set_epi32( 0x00000000, 0x00000000, 0x00000000, 0x80000000 ) );
+	const __m128 vector_float_last_three_sign_bits	= __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x00000000 ) );
+	const __m128 vector_float_first_pos_half		= {   0.5f,   0.0f,   0.0f,   0.0f };	// +.5 0 0 0
+	const __m128 vector_float_first_neg_half		= {  -0.5f,   0.0f,   0.0f,   0.0f };	// -.5 0 0 0
+	const __m128 vector_float_quat2mat_mad1			= {  -1.0f,  -1.0f,  +1.0f,  -1.0f };	//  - - + -
+	const __m128 vector_float_quat2mat_mad2			= {  -1.0f,  +1.0f,  -1.0f,  -1.0f };	//  - + - -
+	const __m128 vector_float_quat2mat_mad3			= {  +1.0f,  -1.0f,  -1.0f,  +1.0f };	//  + - - +
+
+	int i = 0;
+	for ( ; i + 1 < numJoints; i += 2 ) {
+
+		__m128 q0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+0] );
+		__m128 q1 = _mm_load_ps( &jointQuatPtr[i*8+1*8+0] );
+
+		__m128 t0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+4] );
+		__m128 t1 = _mm_load_ps( &jointQuatPtr[i*8+1*8+4] );
+
+		__m128 d0 = _mm_add_ps( q0, q0 );
+		__m128 d1 = _mm_add_ps( q1, q1 );
+
+		__m128 sa0 = _mm_perm_ps( q0, _MM_SHUFFLE( 1, 0, 0, 1 ) );							//   y,   x,   x,   y
+		__m128 sb0 = _mm_perm_ps( d0, _MM_SHUFFLE( 2, 2, 1, 1 ) );							//  y2,  y2,  z2,  z2
+		__m128 sc0 = _mm_perm_ps( q0, _MM_SHUFFLE( 3, 3, 3, 2 ) );							//   z,   w,   w,   w
+		__m128 sd0 = _mm_perm_ps( d0, _MM_SHUFFLE( 0, 1, 2, 2 ) );							//  z2,  z2,  y2,  x2
+		__m128 sa1 = _mm_perm_ps( q1, _MM_SHUFFLE( 1, 0, 0, 1 ) );							//   y,   x,   x,   y
+		__m128 sb1 = _mm_perm_ps( d1, _MM_SHUFFLE( 2, 2, 1, 1 ) );							//  y2,  y2,  z2,  z2
+		__m128 sc1 = _mm_perm_ps( q1, _MM_SHUFFLE( 3, 3, 3, 2 ) );							//   z,   w,   w,   w
+		__m128 sd1 = _mm_perm_ps( d1, _MM_SHUFFLE( 0, 1, 2, 2 ) );							//  z2,  z2,  y2,  x2
+
+		sa0 = _mm_xor_ps( sa0, vector_float_first_sign_bit );
+		sa1 = _mm_xor_ps( sa1, vector_float_first_sign_bit );
+
+		sc0 = _mm_xor_ps( sc0, vector_float_last_three_sign_bits );							// flip stupid inverse quaternions
+		sc1 = _mm_xor_ps( sc1, vector_float_last_three_sign_bits );							// flip stupid inverse quaternions
+
+		__m128 ma0 = _mm_add_ps( _mm_mul_ps( sa0, sb0 ), vector_float_first_pos_half );		//  .5 - yy2,  xy2,  xz2,  yz2		//  .5 0 0 0
+		__m128 mb0 = _mm_add_ps( _mm_mul_ps( sc0, sd0 ), vector_float_first_neg_half );		// -.5 + zz2,  wz2,  wy2,  wx2		// -.5 0 0 0
+		__m128 mc0 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q0, d0 ) );		//  .5 - xx2, -yy2, -zz2, -ww2		//  .5 0 0 0
+		__m128 ma1 = _mm_add_ps( _mm_mul_ps( sa1, sb1 ), vector_float_first_pos_half );		//  .5 - yy2,  xy2,  xz2,  yz2		//  .5 0 0 0
+		__m128 mb1 = _mm_add_ps( _mm_mul_ps( sc1, sd1 ), vector_float_first_neg_half );		// -.5 + zz2,  wz2,  wy2,  wx2		// -.5 0 0 0
+		__m128 mc1 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q1, d1 ) );		//  .5 - xx2, -yy2, -zz2, -ww2		//  .5 0 0 0
+
+		__m128 mf0 = _mm_shuffle_ps( ma0, mc0, _MM_SHUFFLE( 0, 0, 1, 1 ) );					//       xy2,  xy2, .5 - xx2, .5 - xx2	// 01, 01, 10, 10
+		__m128 md0 = _mm_shuffle_ps( mf0, ma0, _MM_SHUFFLE( 3, 2, 0, 2 ) );					//  .5 - xx2,  xy2,  xz2,  yz2			// 10, 01, 02, 03
+		__m128 me0 = _mm_shuffle_ps( ma0, mb0, _MM_SHUFFLE( 3, 2, 1, 0 ) );					//  .5 - yy2,  xy2,  wy2,  wx2			// 00, 01, 12, 13
+		__m128 mf1 = _mm_shuffle_ps( ma1, mc1, _MM_SHUFFLE( 0, 0, 1, 1 ) );					//       xy2,  xy2, .5 - xx2, .5 - xx2	// 01, 01, 10, 10
+		__m128 md1 = _mm_shuffle_ps( mf1, ma1, _MM_SHUFFLE( 3, 2, 0, 2 ) );					//  .5 - xx2,  xy2,  xz2,  yz2			// 10, 01, 02, 03
+		__m128 me1 = _mm_shuffle_ps( ma1, mb1, _MM_SHUFFLE( 3, 2, 1, 0 ) );					//  .5 - yy2,  xy2,  wy2,  wx2			// 00, 01, 12, 13
+
+		__m128 ra0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad1 ), ma0 );		// 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2,					// - - + -
+		__m128 rb0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad2 ), md0 );		// 1 - xx2 - zz2, xy2 + wz2,          , yz2 - wx2		// - + - -
+		__m128 rc0 = _mm_add_ps( _mm_mul_ps( me0, vector_float_quat2mat_mad3 ), md0 );		// 1 - xx2 - yy2,          , xz2 - wy2, yz2 + wx2		// + - - +
+		__m128 ra1 = _mm_add_ps( _mm_mul_ps( mb1, vector_float_quat2mat_mad1 ), ma1 );		// 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2,					// - - + -
+		__m128 rb1 = _mm_add_ps( _mm_mul_ps( mb1, vector_float_quat2mat_mad2 ), md1 );		// 1 - xx2 - zz2, xy2 + wz2,          , yz2 - wx2		// - + - -
+		__m128 rc1 = _mm_add_ps( _mm_mul_ps( me1, vector_float_quat2mat_mad3 ), md1 );		// 1 - xx2 - yy2,          , xz2 - wy2, yz2 + wx2		// + - - +
+
+		__m128 ta0 = _mm_shuffle_ps( ra0, t0, _MM_SHUFFLE( 0, 0, 2, 2 ) );
+		__m128 tb0 = _mm_shuffle_ps( rb0, t0, _MM_SHUFFLE( 1, 1, 3, 3 ) );
+		__m128 tc0 = _mm_shuffle_ps( rc0, t0, _MM_SHUFFLE( 2, 2, 0, 0 ) );
+		__m128 ta1 = _mm_shuffle_ps( ra1, t1, _MM_SHUFFLE( 0, 0, 2, 2 ) );
+		__m128 tb1 = _mm_shuffle_ps( rb1, t1, _MM_SHUFFLE( 1, 1, 3, 3 ) );
+		__m128 tc1 = _mm_shuffle_ps( rc1, t1, _MM_SHUFFLE( 2, 2, 0, 0 ) );
+
+		ra0 = _mm_shuffle_ps( ra0, ta0, _MM_SHUFFLE( 2, 0, 1, 0 ) );						// 00 01 02 10
+		rb0 = _mm_shuffle_ps( rb0, tb0, _MM_SHUFFLE( 2, 0, 0, 1 ) );						// 01 00 03 11
+		rc0 = _mm_shuffle_ps( rc0, tc0, _MM_SHUFFLE( 2, 0, 3, 2 ) );						// 02 03 00 12
+		ra1 = _mm_shuffle_ps( ra1, ta1, _MM_SHUFFLE( 2, 0, 1, 0 ) );						// 00 01 02 10
+		rb1 = _mm_shuffle_ps( rb1, tb1, _MM_SHUFFLE( 2, 0, 0, 1 ) );						// 01 00 03 11
+		rc1 = _mm_shuffle_ps( rc1, tc1, _MM_SHUFFLE( 2, 0, 3, 2 ) );						// 02 03 00 12
+
+		_mm_store_ps( &jointMatPtr[i*12+0*12+0], ra0 );
+		_mm_store_ps( &jointMatPtr[i*12+0*12+4], rb0 );
+		_mm_store_ps( &jointMatPtr[i*12+0*12+8], rc0 );
+		_mm_store_ps( &jointMatPtr[i*12+1*12+0], ra1 );
+		_mm_store_ps( &jointMatPtr[i*12+1*12+4], rb1 );
+		_mm_store_ps( &jointMatPtr[i*12+1*12+8], rc1 );
+	}
+
+	for ( ; i < numJoints; i++ ) {
+
+		__m128 q0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+0] );
+		__m128 t0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+4] );
+
+		__m128 d0 = _mm_add_ps( q0, q0 );
+
+		__m128 sa0 = _mm_perm_ps( q0, _MM_SHUFFLE( 1, 0, 0, 1 ) );							//   y,   x,   x,   y
+		__m128 sb0 = _mm_perm_ps( d0, _MM_SHUFFLE( 2, 2, 1, 1 ) );							//  y2,  y2,  z2,  z2
+		__m128 sc0 = _mm_perm_ps( q0, _MM_SHUFFLE( 3, 3, 3, 2 ) );							//   z,   w,   w,   w
+		__m128 sd0 = _mm_perm_ps( d0, _MM_SHUFFLE( 0, 1, 2, 2 ) );							//  z2,  z2,  y2,  x2
+
+		sa0 = _mm_xor_ps( sa0, vector_float_first_sign_bit );
+		sc0 = _mm_xor_ps( sc0, vector_float_last_three_sign_bits );							// flip stupid inverse quaternions
+
+		__m128 ma0 = _mm_add_ps( _mm_mul_ps( sa0, sb0 ), vector_float_first_pos_half );		//  .5 - yy2,  xy2,  xz2,  yz2		//  .5 0 0 0
+		__m128 mb0 = _mm_add_ps( _mm_mul_ps( sc0, sd0 ), vector_float_first_neg_half );		// -.5 + zz2,  wz2,  wy2,  wx2		// -.5 0 0 0
+		__m128 mc0 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q0, d0 ) );		//  .5 - xx2, -yy2, -zz2, -ww2		//  .5 0 0 0
+
+		__m128 mf0 = _mm_shuffle_ps( ma0, mc0, _MM_SHUFFLE( 0, 0, 1, 1 ) );					//       xy2,  xy2, .5 - xx2, .5 - xx2	// 01, 01, 10, 10
+		__m128 md0 = _mm_shuffle_ps( mf0, ma0, _MM_SHUFFLE( 3, 2, 0, 2 ) );					//  .5 - xx2,  xy2,  xz2,  yz2			// 10, 01, 02, 03
+		__m128 me0 = _mm_shuffle_ps( ma0, mb0, _MM_SHUFFLE( 3, 2, 1, 0 ) );					//  .5 - yy2,  xy2,  wy2,  wx2			// 00, 01, 12, 13
+
+		__m128 ra0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad1 ), ma0 );		// 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2,					// - - + -
+		__m128 rb0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad2 ), md0 );		// 1 - xx2 - zz2, xy2 + wz2,          , yz2 - wx2		// - + - -
+		__m128 rc0 = _mm_add_ps( _mm_mul_ps( me0, vector_float_quat2mat_mad3 ), md0 );		// 1 - xx2 - yy2,          , xz2 - wy2, yz2 + wx2		// + - - +
+
+		__m128 ta0 = _mm_shuffle_ps( ra0, t0, _MM_SHUFFLE( 0, 0, 2, 2 ) );
+		__m128 tb0 = _mm_shuffle_ps( rb0, t0, _MM_SHUFFLE( 1, 1, 3, 3 ) );
+		__m128 tc0 = _mm_shuffle_ps( rc0, t0, _MM_SHUFFLE( 2, 2, 0, 0 ) );
+
+		ra0 = _mm_shuffle_ps( ra0, ta0, _MM_SHUFFLE( 2, 0, 1, 0 ) );						// 00 01 02 10
+		rb0 = _mm_shuffle_ps( rb0, tb0, _MM_SHUFFLE( 2, 0, 0, 1 ) );						// 01 00 03 11
+		rc0 = _mm_shuffle_ps( rc0, tc0, _MM_SHUFFLE( 2, 0, 3, 2 ) );						// 02 03 00 12
+
+		_mm_store_ps( &jointMatPtr[i*12+0*12+0], ra0 );
+		_mm_store_ps( &jointMatPtr[i*12+0*12+4], rb0 );
+		_mm_store_ps( &jointMatPtr[i*12+0*12+8], rc0 );
+	}
+}
+
+/*
+============
+idSIMD_SSE::ConvertJointMatsToJointQuats
+============
+*/
+void VPCALL idSIMD_SSE::ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints ) {
+
+	assert( sizeof( idJointQuat ) == JOINTQUAT_SIZE );
+	assert( sizeof( idJointMat ) == JOINTMAT_SIZE );
+	assert( (int)(&((idJointQuat *)0)->t) == (int)(&((idJointQuat *)0)->q) + (int)sizeof( ((idJointQuat *)0)->q ) );
+
+	const __m128 vector_float_zero		= _mm_setzero_ps();
+	const __m128 vector_float_one		= { 1.0f, 1.0f, 1.0f, 1.0f };
+	const __m128 vector_float_not		= __m128c( _mm_set_epi32( -1, -1, -1, -1 ) );
+	const __m128 vector_float_sign_bit	= __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
+	const __m128 vector_float_rsqrt_c0	= {  -3.0f,  -3.0f,  -3.0f,  -3.0f };
+	const __m128 vector_float_rsqrt_c2	= { -0.25f, -0.25f, -0.25f, -0.25f };
+
+	int i = 0;
+	for ( ; i < numJoints - 3; i += 4 ) {
+		const float *__restrict m = (float *)&jointMats[i];
+		float *__restrict q = (float *)&jointQuats[i];
+
+		__m128 ma0 = _mm_load_ps( &m[0*12+0] );
+		__m128 ma1 = _mm_load_ps( &m[0*12+4] );
+		__m128 ma2 = _mm_load_ps( &m[0*12+8] );
+
+		__m128 mb0 = _mm_load_ps( &m[1*12+0] );
+		__m128 mb1 = _mm_load_ps( &m[1*12+4] );
+		__m128 mb2 = _mm_load_ps( &m[1*12+8] );
+
+		__m128 mc0 = _mm_load_ps( &m[2*12+0] );
+		__m128 mc1 = _mm_load_ps( &m[2*12+4] );
+		__m128 mc2 = _mm_load_ps( &m[2*12+8] );
+
+		__m128 md0 = _mm_load_ps( &m[3*12+0] );
+		__m128 md1 = _mm_load_ps( &m[3*12+4] );
+		__m128 md2 = _mm_load_ps( &m[3*12+8] );
+
+		__m128 ta0 = _mm_unpacklo_ps( ma0, mc0 );	// a0, c0, a1, c1
+		__m128 ta1 = _mm_unpackhi_ps( ma0, mc0 );	// a2, c2, a3, c3
+		__m128 ta2 = _mm_unpacklo_ps( mb0, md0 );	// b0, d0, b1, b2
+		__m128 ta3 = _mm_unpackhi_ps( mb0, md0 );	// b2, d2, b3, d3
+
+		__m128 tb0 = _mm_unpacklo_ps( ma1, mc1 );	// a0, c0, a1, c1
+		__m128 tb1 = _mm_unpackhi_ps( ma1, mc1 );	// a2, c2, a3, c3
+		__m128 tb2 = _mm_unpacklo_ps( mb1, md1 );	// b0, d0, b1, b2
+		__m128 tb3 = _mm_unpackhi_ps( mb1, md1 );	// b2, d2, b3, d3
+
+		__m128 tc0 = _mm_unpacklo_ps( ma2, mc2 );	// a0, c0, a1, c1
+		__m128 tc1 = _mm_unpackhi_ps( ma2, mc2 );	// a2, c2, a3, c3
+		__m128 tc2 = _mm_unpacklo_ps( mb2, md2 );	// b0, d0, b1, b2
+		__m128 tc3 = _mm_unpackhi_ps( mb2, md2 );	// b2, d2, b3, d3
+
+		__m128 m00 = _mm_unpacklo_ps( ta0, ta2 );
+		__m128 m01 = _mm_unpackhi_ps( ta0, ta2 );
+		__m128 m02 = _mm_unpacklo_ps( ta1, ta3 );
+		__m128 m03 = _mm_unpackhi_ps( ta1, ta3 );
+
+		__m128 m10 = _mm_unpacklo_ps( tb0, tb2 );
+		__m128 m11 = _mm_unpackhi_ps( tb0, tb2 );
+		__m128 m12 = _mm_unpacklo_ps( tb1, tb3 );
+		__m128 m13 = _mm_unpackhi_ps( tb1, tb3 );
+
+		__m128 m20 = _mm_unpacklo_ps( tc0, tc2 );
+		__m128 m21 = _mm_unpackhi_ps( tc0, tc2 );
+		__m128 m22 = _mm_unpacklo_ps( tc1, tc3 );
+		__m128 m23 = _mm_unpackhi_ps( tc1, tc3 );
+
+		__m128 b00 = _mm_add_ps( m00, m11 );
+		__m128 b11 = _mm_cmpgt_ps( m00, m22 );
+		__m128 b01 = _mm_add_ps( b00, m22 );
+		__m128 b10 = _mm_cmpgt_ps( m00, m11 );
+		__m128 b0  = _mm_cmpgt_ps( b01, vector_float_zero );
+		__m128 b1  = _mm_and_ps( b10, b11 );
+		__m128 b2  = _mm_cmpgt_ps( m11, m22 );
+
+		__m128 m0  = b0;
+		__m128 m1  = _mm_and_ps( _mm_xor_ps( b0, vector_float_not ), b1 );
+		__m128 p1  = _mm_or_ps( b0, b1 );
+		__m128 p2  = _mm_or_ps( p1, b2 );
+		__m128 m2  = _mm_and_ps( _mm_xor_ps( p1, vector_float_not ), b2 );
+		__m128 m3  = _mm_xor_ps( p2, vector_float_not );
+
+		__m128 i0  = _mm_or_ps( m2, m3 );
+		__m128 i1  = _mm_or_ps( m1, m3 );
+		__m128 i2  = _mm_or_ps( m1, m2 );
+
+		__m128 s0  = _mm_and_ps( i0, vector_float_sign_bit );
+		__m128 s1  = _mm_and_ps( i1, vector_float_sign_bit );
+		__m128 s2  = _mm_and_ps( i2, vector_float_sign_bit );
+
+		m00 = _mm_xor_ps( m00, s0 );
+		m11 = _mm_xor_ps( m11, s1 );
+		m22 = _mm_xor_ps( m22, s2 );
+		m21 = _mm_xor_ps( m21, s0 );
+		m02 = _mm_xor_ps( m02, s1 );
+		m10 = _mm_xor_ps( m10, s2 );
+
+		__m128 t0  = _mm_add_ps( m00, m11 );
+		__m128 t1  = _mm_add_ps( m22, vector_float_one );
+		__m128 q0  = _mm_add_ps( t0, t1 );
+		__m128 q1  = _mm_sub_ps( m01, m10 );
+		__m128 q2  = _mm_sub_ps( m20, m02 );
+		__m128 q3  = _mm_sub_ps( m12, m21 );
+
+		__m128 rs = _mm_rsqrt_ps( q0 );
+		__m128 sq = _mm_mul_ps( rs, rs );
+		__m128 sh = _mm_mul_ps( rs, vector_float_rsqrt_c2 );
+		__m128 sx = _mm_madd_ps( q0, sq, vector_float_rsqrt_c0 );
+		__m128 s  = _mm_mul_ps( sh, sx );
+
+		q0 = _mm_mul_ps( q0, s );
+		q1 = _mm_mul_ps( q1, s );
+		q2 = _mm_mul_ps( q2, s );
+		q3 = _mm_mul_ps( q3, s );
+
+		m0 = _mm_or_ps( m0, m2 );
+		m2 = _mm_or_ps( m2, m3 );
+
+		__m128 fq0 = _mm_sel_ps( q0, q3, m0 );
+		__m128 fq1 = _mm_sel_ps( q1, q2, m0 );
+		__m128 fq2 = _mm_sel_ps( q2, q1, m0 );
+		__m128 fq3 = _mm_sel_ps( q3, q0, m0 );
+
+		__m128 rq0 = _mm_sel_ps( fq0, fq2, m2 );
+		__m128 rq1 = _mm_sel_ps( fq1, fq3, m2 );
+		__m128 rq2 = _mm_sel_ps( fq2, fq0, m2 );
+		__m128 rq3 = _mm_sel_ps( fq3, fq1, m2 );
+
+		__m128 tq0 = _mm_unpacklo_ps( rq0, rq2 );
+		__m128 tq1 = _mm_unpackhi_ps( rq0, rq2 );
+		__m128 tq2 = _mm_unpacklo_ps( rq1, rq3 );
+		__m128 tq3 = _mm_unpackhi_ps( rq1, rq3 );
+
+		__m128 sq0 = _mm_unpacklo_ps( tq0, tq2 );
+		__m128 sq1 = _mm_unpackhi_ps( tq0, tq2 );
+		__m128 sq2 = _mm_unpacklo_ps( tq1, tq3 );
+		__m128 sq3 = _mm_unpackhi_ps( tq1, tq3 );
+
+		__m128 tt0 = _mm_unpacklo_ps( m03, m23 );
+		__m128 tt1 = _mm_unpackhi_ps( m03, m23 );
+		__m128 tt2 = _mm_unpacklo_ps( m13, vector_float_zero );
+		__m128 tt3 = _mm_unpackhi_ps( m13, vector_float_zero );
+
+		__m128 st0 = _mm_unpacklo_ps( tt0, tt2 );
+		__m128 st1 = _mm_unpackhi_ps( tt0, tt2 );
+		__m128 st2 = _mm_unpacklo_ps( tt1, tt3 );
+		__m128 st3 = _mm_unpackhi_ps( tt1, tt3 );
+
+		_mm_store_ps( &q[0*4], sq0 );
+		_mm_store_ps( &q[1*4], st0 );
+		_mm_store_ps( &q[2*4], sq1 );
+		_mm_store_ps( &q[3*4], st1 );
+		_mm_store_ps( &q[4*4], sq2 );
+		_mm_store_ps( &q[5*4], st2 );
+		_mm_store_ps( &q[6*4], sq3 );
+		_mm_store_ps( &q[7*4], st3 );
+	}
+
+	float sign[2] = { 1.0f, -1.0f };
+
+	for ( ; i < numJoints; i++ ) {
+		const float *__restrict m = (float *)&jointMats[i];
+		float *__restrict q = (float *)&jointQuats[i];
+
+		int b0 = m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0f;
+		int b1 = m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2];
+		int b2 = m[1 * 4 + 1] > m[2 * 4 + 2];
+
+		int m0 = b0;
+		int m1 = ( !b0 ) & b1;
+		int m2 = ( !( b0 | b1 ) ) & b2;
+		int m3 = !( b0 | b1 | b2 );
+
+		int i0 = ( m2 | m3 );
+		int i1 = ( m1 | m3 );
+		int i2 = ( m1 | m2 );
+
+		float s0 = sign[i0];
+		float s1 = sign[i1];
+		float s2 = sign[i2];
+
+		float t = s0 * m[0 * 4 + 0] + s1 * m[1 * 4 + 1] + s2 * m[2 * 4 + 2] + 1.0f;
+		float s = __frsqrts( t );
+		s = ( t * s * s + -3.0f ) * ( s * -0.25f );
+
+		q[0] = t * s;
+		q[1] = ( m[0 * 4 + 1] - s2 * m[1 * 4 + 0] ) * s;
+		q[2] = ( m[2 * 4 + 0] - s1 * m[0 * 4 + 2] ) * s;
+		q[3] = ( m[1 * 4 + 2] - s0 * m[2 * 4 + 1] ) * s;
+
+		if ( m0 | m2 ) {
+			// reverse
+			SwapValues( q[0], q[3] );
+			SwapValues( q[1], q[2] );
+		}
+		if ( m2 | m3 ) {
+			// rotate 2
+			SwapValues( q[0], q[2] );
+			SwapValues( q[1], q[3] );
+		}
+
+		q[4] = m[0 * 4 + 3];
+		q[5] = m[1 * 4 + 3];
+		q[6] = m[2 * 4 + 3];
+		q[7] = 0.0f;
+	}
+}
+
+/*
+============
+idSIMD_SSE::TransformJoints
+============
+*/
+void VPCALL idSIMD_SSE::TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
+	const __m128 vector_float_mask_keep_last	= __m128c( _mm_set_epi32( 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000 ) );
+
+	const float *__restrict firstMatrix = jointMats->ToFloatPtr() + ( firstJoint + firstJoint + firstJoint - 3 ) * 4;
+
+	__m128 pma = _mm_load_ps( firstMatrix + 0 );
+	__m128 pmb = _mm_load_ps( firstMatrix + 4 );
+	__m128 pmc = _mm_load_ps( firstMatrix + 8 );
+
+	for ( int joint = firstJoint; joint <= lastJoint; joint++ ) {
+		const int parent = parents[joint];
+		const float *__restrict parentMatrix = jointMats->ToFloatPtr() + ( parent + parent + parent ) * 4;
+		float *__restrict childMatrix = jointMats->ToFloatPtr() + ( joint + joint + joint ) * 4;
+
+		if ( parent != joint - 1 ) {
+			pma = _mm_load_ps( parentMatrix + 0 );
+			pmb = _mm_load_ps( parentMatrix + 4 );
+			pmc = _mm_load_ps( parentMatrix + 8 );
+		}
+
+		__m128 cma = _mm_load_ps( childMatrix + 0 );
+		__m128 cmb = _mm_load_ps( childMatrix + 4 );
+		__m128 cmc = _mm_load_ps( childMatrix + 8 );
+
+		__m128 ta = _mm_splat_ps( pma, 0 );
+		__m128 tb = _mm_splat_ps( pmb, 0 );
+		__m128 tc = _mm_splat_ps( pmc, 0 );
+
+		__m128 td = _mm_splat_ps( pma, 1 );
+		__m128 te = _mm_splat_ps( pmb, 1 );
+		__m128 tf = _mm_splat_ps( pmc, 1 );
+
+		__m128 tg = _mm_splat_ps( pma, 2 );
+		__m128 th = _mm_splat_ps( pmb, 2 );
+		__m128 ti = _mm_splat_ps( pmc, 2 );
+
+		pma = _mm_madd_ps( ta, cma, _mm_and_ps( pma, vector_float_mask_keep_last ) );
+		pmb = _mm_madd_ps( tb, cma, _mm_and_ps( pmb, vector_float_mask_keep_last ) );
+		pmc = _mm_madd_ps( tc, cma, _mm_and_ps( pmc, vector_float_mask_keep_last ) );
+
+		pma = _mm_madd_ps( td, cmb, pma );
+		pmb = _mm_madd_ps( te, cmb, pmb );
+		pmc = _mm_madd_ps( tf, cmb, pmc );
+
+		pma = _mm_madd_ps( tg, cmc, pma );
+		pmb = _mm_madd_ps( th, cmc, pmb );
+		pmc = _mm_madd_ps( ti, cmc, pmc );
+
+		_mm_store_ps( childMatrix + 0, pma );
+		_mm_store_ps( childMatrix + 4, pmb );
+		_mm_store_ps( childMatrix + 8, pmc );
+	}
+}
+
+/*
+============
+idSIMD_SSE::UntransformJoints
+============
+*/
+void VPCALL idSIMD_SSE::UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
+	const __m128 vector_float_mask_keep_last	= __m128c( _mm_set_epi32( 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000 ) );
+
+	for ( int joint = lastJoint; joint >= firstJoint; joint-- ) {
+		assert( parents[joint] < joint );
+		const int parent = parents[joint];
+		const float *__restrict parentMatrix = jointMats->ToFloatPtr() + ( parent + parent + parent ) * 4;
+		float *__restrict childMatrix = jointMats->ToFloatPtr() + ( joint + joint + joint ) * 4;
+
+		__m128 pma = _mm_load_ps( parentMatrix + 0 );
+		__m128 pmb = _mm_load_ps( parentMatrix + 4 );
+		__m128 pmc = _mm_load_ps( parentMatrix + 8 );
+
+		__m128 cma = _mm_load_ps( childMatrix + 0 );
+		__m128 cmb = _mm_load_ps( childMatrix + 4 );
+		__m128 cmc = _mm_load_ps( childMatrix + 8 );
+
+		__m128 ta = _mm_splat_ps( pma, 0 );
+		__m128 tb = _mm_splat_ps( pma, 1 );
+		__m128 tc = _mm_splat_ps( pma, 2 );
+
+		__m128 td = _mm_splat_ps( pmb, 0 );
+		__m128 te = _mm_splat_ps( pmb, 1 );
+		__m128 tf = _mm_splat_ps( pmb, 2 );
+
+		__m128 tg = _mm_splat_ps( pmc, 0 );
+		__m128 th = _mm_splat_ps( pmc, 1 );
+		__m128 ti = _mm_splat_ps( pmc, 2 );
+
+		cma = _mm_sub_ps( cma, _mm_and_ps( pma, vector_float_mask_keep_last ) );
+		cmb = _mm_sub_ps( cmb, _mm_and_ps( pmb, vector_float_mask_keep_last ) );
+		cmc = _mm_sub_ps( cmc, _mm_and_ps( pmc, vector_float_mask_keep_last ) );
+
+		pma = _mm_mul_ps( ta, cma );
+		pmb = _mm_mul_ps( tb, cma );
+		pmc = _mm_mul_ps( tc, cma );
+
+		pma = _mm_madd_ps( td, cmb, pma );
+		pmb = _mm_madd_ps( te, cmb, pmb );
+		pmc = _mm_madd_ps( tf, cmb, pmc );
+
+		pma = _mm_madd_ps( tg, cmc, pma );
+		pmb = _mm_madd_ps( th, cmc, pmb );
+		pmc = _mm_madd_ps( ti, cmc, pmc );
+
+		_mm_store_ps( childMatrix + 0, pma );
+		_mm_store_ps( childMatrix + 4, pmb );
+		_mm_store_ps( childMatrix + 8, pmc );
+	}
+}
+

neo/idlib/math/Simd_SSE.h

@@ -0,0 +1,52 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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_Generic {
+public:
+	virtual const char * VPCALL GetName() const;
+
+	virtual void VPCALL BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints );
+	virtual void VPCALL BlendJointsFast( 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 );
+};
+
+#endif /* !__MATH_SIMD_SSE_H__ */

neo/idlib/math/VecX.cpp

@@ -0,0 +1,49 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+//===============================================================
+//
+//	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 );
+}

neo/idlib/math/VecX.h

@@ -0,0 +1,802 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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_VECX_H__
+#define __MATH_VECX_H__
+
+/*
+===============================================================================
+
+idVecX - arbitrary sized vector
+
+The vector lives on 16 byte aligned and 16 byte padded memory.
+
+NOTE: due to the temporary memory pool idVecX cannot be used by multiple threads
+
+===============================================================================
+*/
+
+#define VECX_MAX_TEMP		1024
+#define VECX_QUAD( x )		( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
+#define VECX_CLEAREND()		int s = size; while( s < ( ( s + 3) & ~3 ) ) { p[s++] = 0.0f; }
+#define VECX_ALLOCA( n )	( (float *) _alloca16( VECX_QUAD( n ) ) )
+#define VECX_SIMD
+
+class idVecX {
+	friend class idMatX;
+
+public:	
+	ID_INLINE					idVecX();
+	ID_INLINE					explicit idVecX( int length );
+	ID_INLINE					explicit idVecX( int length, float *data );
+	ID_INLINE					~idVecX();
+
+	ID_INLINE	float			Get( int index ) const;
+	ID_INLINE	float &			Get( int index );
+
+	ID_INLINE	float			operator[]( const int index ) const;
+	ID_INLINE	float &			operator[]( const int index );
+	ID_INLINE	idVecX			operator-() const;
+	ID_INLINE	idVecX &		operator=( const idVecX &a );
+	ID_INLINE	idVecX			operator*( const float a ) const;
+	ID_INLINE	idVecX			operator/( const float a ) const;
+	ID_INLINE	float			operator*( const idVecX &a ) const;
+	ID_INLINE	idVecX			operator-( const idVecX &a ) const;
+	ID_INLINE	idVecX			operator+( const idVecX &a ) const;
+	ID_INLINE	idVecX &		operator*=( const float a );
+	ID_INLINE	idVecX &		operator/=( const float a );
+	ID_INLINE	idVecX &		operator+=( const idVecX &a );
+	ID_INLINE	idVecX &		operator-=( const idVecX &a );
+
+	friend ID_INLINE	idVecX	operator*( const float a, const idVecX &b );
+
+	ID_INLINE	bool			Compare( const idVecX &a ) const;							// exact compare, no epsilon
+	ID_INLINE	bool			Compare( const idVecX &a, const float epsilon ) const;		// compare with epsilon
+	ID_INLINE	bool			operator==(	const idVecX &a ) const;						// exact compare, no epsilon
+	ID_INLINE	bool			operator!=(	const idVecX &a ) const;						// exact compare, no epsilon
+
+	ID_INLINE	void			SetSize( int size );
+	ID_INLINE	void			ChangeSize( int size, bool makeZero = false );
+	ID_INLINE	int				GetSize() const { return size; }
+	ID_INLINE	void			SetData( int length, float *data );
+	ID_INLINE	void			Zero();
+	ID_INLINE	void			Zero( int length );
+	ID_INLINE	void			Random( int seed, float l = 0.0f, float u = 1.0f );
+	ID_INLINE	void			Random( int length, int seed, float l = 0.0f, float u = 1.0f );
+	ID_INLINE	void			Negate();
+	ID_INLINE	void			Clamp( float min, float max );
+	ID_INLINE	idVecX &		SwapElements( int e1, int e2 );
+
+	ID_INLINE	float			Length() const;
+	ID_INLINE	float			LengthSqr() const;
+	ID_INLINE	idVecX			Normalize() const;
+	ID_INLINE	float			NormalizeSelf();
+
+	ID_INLINE	int				GetDimension() const;
+
+	ID_INLINE	void			AddScaleAdd( const float scale, const idVecX & v0, const idVecX & v1 );
+
+	ID_INLINE	const idVec3 &	SubVec3( int index ) const;
+	ID_INLINE	idVec3 &		SubVec3( int index );
+	ID_INLINE	const idVec6 &	SubVec6( int index = 0 ) const;
+	ID_INLINE	idVec6 &		SubVec6( int index = 0 );
+	ID_INLINE	const float *	ToFloatPtr() const;
+	ID_INLINE	float *			ToFloatPtr();
+	const char *	ToString( int precision = 2 ) const;
+
+private:
+	int				size;					// size of the vector
+	int				alloced;				// if -1 p points to data set with SetData
+	float *			p;						// memory the vector is stored
+
+	static float	temp[VECX_MAX_TEMP+4];	// used to store intermediate results
+	static float *	tempPtr;				// pointer to 16 byte aligned temporary memory
+	static int		tempIndex;				// index into memory pool, wraps around
+
+	ID_INLINE void	SetTempSize( int size );
+};
+
+
+/*
+========================
+idVecX::idVecX
+========================
+*/
+ID_INLINE idVecX::idVecX() {
+	size = alloced = 0;
+	p = NULL;
+}
+
+/*
+========================
+idVecX::idVecX
+========================
+*/
+ID_INLINE idVecX::idVecX( int length ) {
+	size = alloced = 0;
+	p = NULL;
+	SetSize( length );
+}
+
+/*
+========================
+idVecX::idVecX
+========================
+*/
+ID_INLINE idVecX::idVecX( int length, float *data ) {
+	size = alloced = 0;
+	p = NULL;
+	SetData( length, data );
+}
+
+/*
+========================
+idVecX::~idVecX
+========================
+*/
+ID_INLINE idVecX::~idVecX() {
+	// if not temp memory
+	if ( p && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
+		Mem_Free16( p );
+	}
+}
+
+/*
+========================
+idVecX::Get
+========================
+*/
+ID_INLINE float idVecX::Get( int index ) const {
+	assert( index >= 0 && index < size );
+	return p[index];
+}
+
+/*
+========================
+idVecX::Get
+========================
+*/
+ID_INLINE float & idVecX::Get( int index ) {
+	assert( index >= 0 && index < size );
+	return p[index];
+}
+
+/*
+========================
+idVecX::operator[]
+========================
+*/
+ID_INLINE float idVecX::operator[]( int index ) const {
+	return Get( index );
+}
+
+/*
+========================
+idVecX::operator[]
+========================
+*/
+ID_INLINE float & idVecX::operator[]( int index ) {
+	return Get( index );
+}
+
+/*
+========================
+idVecX::operator-
+========================
+*/
+ID_INLINE idVecX idVecX::operator-() const {
+	idVecX m;
+
+	m.SetTempSize( size );
+#ifdef VECX_SIMD
+	ALIGN16( unsigned int signBit[4] ) = { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( m.p + i, _mm_xor_ps( _mm_load_ps( p + i ), (__m128 &) signBit[0] ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		m.p[i] = -p[i];
+	}
+#endif
+	return m;
+}
+
+/*
+========================
+idVecX::operator=
+========================
+*/
+ID_INLINE idVecX &idVecX::operator=( const idVecX &a ) { 
+	SetSize( a.size );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < a.size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_load_ps( a.p + i ) );
+	}
+#else
+	memcpy( p, a.p, a.size * sizeof( float ) );
+#endif
+	idVecX::tempIndex = 0;
+	return *this;
+}
+
+/*
+========================
+idVecX::operator+
+========================
+*/
+ID_INLINE idVecX idVecX::operator+( const idVecX &a ) const {
+	idVecX m;
+
+	assert( size == a.size );
+	m.SetTempSize( size );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( m.p + i, _mm_add_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		m.p[i] = p[i] + a.p[i];
+	}
+#endif
+	return m;
+}
+
+/*
+========================
+idVecX::operator-
+========================
+*/
+ID_INLINE idVecX idVecX::operator-( const idVecX &a ) const {
+	idVecX m;
+
+	assert( size == a.size );
+	m.SetTempSize( size );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( m.p + i, _mm_sub_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		m.p[i] = p[i] - a.p[i];
+	}
+#endif
+	return m;
+}
+
+/*
+========================
+idVecX::operator+=
+========================
+*/
+ID_INLINE idVecX &idVecX::operator+=( const idVecX &a ) {
+	assert( size == a.size );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_add_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		p[i] += a.p[i];
+	}
+#endif
+	idVecX::tempIndex = 0;
+	return *this;
+}
+
+/*
+========================
+idVecX::operator-=
+========================
+*/
+ID_INLINE idVecX &idVecX::operator-=( const idVecX &a ) {
+	assert( size == a.size );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_sub_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		p[i] -= a.p[i];
+	}
+#endif
+	idVecX::tempIndex = 0;
+	return *this;
+}
+
+/*
+========================
+idVecX::operator*
+========================
+*/
+ID_INLINE idVecX idVecX::operator*( const float a ) const {
+	idVecX m;
+
+	m.SetTempSize( size );
+#ifdef VECX_SIMD
+	__m128 va = _mm_load1_ps( & a );
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( m.p + i, _mm_mul_ps( _mm_load_ps( p + i ), va ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		m.p[i] = p[i] * a;
+	}
+#endif
+	return m;
+}
+
+/*
+========================
+idVecX::operator*=
+========================
+*/
+ID_INLINE idVecX &idVecX::operator*=( const float a ) {
+#ifdef VECX_SIMD
+	__m128 va = _mm_load1_ps( & a );
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_mul_ps( _mm_load_ps( p + i ), va ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		p[i] *= a;
+	}
+#endif
+	return *this;
+}
+
+/*
+========================
+idVecX::operator/
+========================
+*/
+ID_INLINE idVecX idVecX::operator/( const float a ) const {
+	assert( fabs( a ) > idMath::FLT_SMALLEST_NON_DENORMAL );
+	return (*this) * ( 1.0f / a );
+}
+
+/*
+========================
+idVecX::operator/=
+========================
+*/
+ID_INLINE idVecX &idVecX::operator/=( const float a ) {
+	assert( fabs( a ) > idMath::FLT_SMALLEST_NON_DENORMAL );
+	(*this) *= ( 1.0f / a );
+	return *this;
+}
+
+/*
+========================
+operator*
+========================
+*/
+ID_INLINE idVecX operator*( const float a, const idVecX &b ) {
+	return b * a;
+}
+
+/*
+========================
+idVecX::operator*
+========================
+*/
+ID_INLINE float idVecX::operator*( const idVecX &a ) const {
+	assert( size == a.size );
+	float sum = 0.0f;
+	for ( int i = 0; i < size; i++ ) {
+		sum += p[i] * a.p[i];
+	}
+	return sum;
+}
+
+/*
+========================
+idVecX::Compare
+========================
+*/
+ID_INLINE bool idVecX::Compare( const idVecX &a ) const {
+	assert( size == a.size );
+	for ( int i = 0; i < size; i++ ) {
+		if ( p[i] != a.p[i] ) {
+			return false;
+		}
+	}
+	return true;
+}
+
+/*
+========================
+idVecX::Compare
+========================
+*/
+ID_INLINE bool idVecX::Compare( const idVecX &a, const float epsilon ) const {
+	assert( size == a.size );
+	for ( int i = 0; i < size; i++ ) {
+		if ( idMath::Fabs( p[i] - a.p[i] ) > epsilon ) {
+			return false;
+		}
+	}
+	return true;
+}
+
+/*
+========================
+idVecX::operator==
+========================
+*/
+ID_INLINE bool idVecX::operator==( const idVecX &a ) const {
+	return Compare( a );
+}
+
+/*
+========================
+idVecX::operator!=
+========================
+*/
+ID_INLINE bool idVecX::operator!=( const idVecX &a ) const {
+	return !Compare( a );
+}
+
+/*
+========================
+idVecX::SetSize
+========================
+*/
+ID_INLINE void idVecX::SetSize( int newSize ) {
+	//assert( p < idVecX::tempPtr || p > idVecX::tempPtr + VECX_MAX_TEMP );
+	if ( newSize != size || p == NULL ) {
+		int alloc = ( newSize + 3 ) & ~3;
+		if ( alloc > alloced && alloced != -1 ) {
+			if ( p ) {
+				Mem_Free16( p );
+			}
+			p = (float *) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
+			alloced = alloc;
+		}
+		size = newSize;
+		VECX_CLEAREND();
+	}
+}
+
+/*
+========================
+idVecX::ChangeSize
+========================
+*/
+ID_INLINE void idVecX::ChangeSize( int newSize, bool makeZero ) {
+	if ( newSize != size ) {
+		int alloc = ( newSize + 3 ) & ~3;
+		if ( alloc > alloced && alloced != -1 ) {
+			float *oldVec = p;
+			p = (float *) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
+			alloced = alloc;
+			if ( oldVec ) {
+				for ( int i = 0; i < size; i++ ) {
+					p[i] = oldVec[i];
+				}
+				Mem_Free16( oldVec );
+			}
+			if ( makeZero ) {
+				// zero any new elements
+				for ( int i = size; i < newSize; i++ ) {
+					p[i] = 0.0f;
+				}
+			}
+		}
+		size = newSize;
+		VECX_CLEAREND();
+	}
+}
+
+/*
+========================
+idVecX::SetTempSize
+========================
+*/
+ID_INLINE void idVecX::SetTempSize( int newSize ) {
+	size = newSize;
+	alloced = ( newSize + 3 ) & ~3;
+	assert( alloced < VECX_MAX_TEMP );
+	if ( idVecX::tempIndex + alloced > VECX_MAX_TEMP ) {
+		idVecX::tempIndex = 0;
+	}
+	p = idVecX::tempPtr + idVecX::tempIndex;
+	idVecX::tempIndex += alloced;
+	VECX_CLEAREND();
+}
+
+/*
+========================
+idVecX::SetData
+========================
+*/
+ID_INLINE void idVecX::SetData( int length, float *data ) {
+	if ( p != NULL && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
+		Mem_Free16( p );
+	}
+	assert_16_byte_aligned( data ); // data must be 16 byte aligned
+	p = data;
+	size = length;
+	alloced = -1;
+	VECX_CLEAREND();
+}
+
+/*
+========================
+idVecX::Zero
+========================
+*/
+ID_INLINE void idVecX::Zero() {
+#ifdef VECX_SIMD
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_setzero_ps() );
+	}
+#else
+	memset( p, 0, size * sizeof( float ) );
+#endif
+}
+
+/*
+========================
+idVecX::Zero
+========================
+*/
+ID_INLINE void idVecX::Zero( int length ) {
+	SetSize( length );
+#ifdef VECX_SIMD
+	for ( int i = 0; i < length; i += 4 ) {
+		_mm_store_ps( p + i, _mm_setzero_ps() );
+	}
+#else
+	memset( p, 0, length * sizeof( float ) );
+#endif
+}
+
+/*
+========================
+idVecX::Random
+========================
+*/
+ID_INLINE void idVecX::Random( int seed, float l, float u ) {
+	idRandom rnd( seed );
+
+	float c = u - l;
+	for ( int i = 0; i < size; i++ ) {
+		p[i] = l + rnd.RandomFloat() * c;
+	}
+}
+
+/*
+========================
+idVecX::Random
+========================
+*/
+ID_INLINE void idVecX::Random( int length, int seed, float l, float u ) {
+	idRandom rnd( seed );
+
+	SetSize( length );
+	float c = u - l;
+	for ( int i = 0; i < size; i++ ) {
+		p[i] = l + rnd.RandomFloat() * c;
+	}
+}
+
+/*
+========================
+idVecX::Negate
+========================
+*/
+ID_INLINE void idVecX::Negate() {
+#ifdef VECX_SIMD
+	ALIGN16( const unsigned int signBit[4] ) = { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
+	for ( int i = 0; i < size; i += 4 ) {
+		_mm_store_ps( p + i, _mm_xor_ps( _mm_load_ps( p + i ), (__m128 &) signBit[0] ) );
+	}
+#else
+	for ( int i = 0; i < size; i++ ) {
+		p[i] = -p[i];
+	}
+#endif
+}
+
+/*
+========================
+idVecX::Clamp
+========================
+*/
+ID_INLINE void idVecX::Clamp( float min, float max ) {
+	for ( int i = 0; i < size; i++ ) {
+		if ( p[i] < min ) {
+			p[i] = min;
+		} else if ( p[i] > max ) {
+			p[i] = max;
+		}
+	}
+}
+
+/*
+========================
+idVecX::SwapElements
+========================
+*/
+ID_INLINE idVecX &idVecX::SwapElements( int e1, int e2 ) {
+	float tmp;
+	tmp = p[e1];
+	p[e1] = p[e2];
+	p[e2] = tmp;
+	return *this;
+}
+
+/*
+========================
+idVecX::Length
+========================
+*/
+ID_INLINE float idVecX::Length() const {
+	float sum = 0.0f;
+	for ( int i = 0; i < size; i++ ) {
+		sum += p[i] * p[i];
+	}
+	return idMath::Sqrt( sum );
+}
+
+/*
+========================
+idVecX::LengthSqr
+========================
+*/
+ID_INLINE float idVecX::LengthSqr() const {
+	float sum = 0.0f;
+	for ( int i = 0; i < size; i++ ) {
+		sum += p[i] * p[i];
+	}
+	return sum;
+}
+
+/*
+========================
+idVecX::Normalize
+========================
+*/
+ID_INLINE idVecX idVecX::Normalize() const {
+	idVecX m;
+
+	m.SetTempSize( size );
+	float sum = 0.0f;
+	for ( int i = 0; i < size; i++ ) {
+		sum += p[i] * p[i];
+	}
+	float invSqrt = idMath::InvSqrt( sum );
+	for ( int i = 0; i < size; i++ ) {
+		m.p[i] = p[i] * invSqrt;
+	}
+	return m;
+}
+
+/*
+========================
+idVecX::NormalizeSelf
+========================
+*/
+ID_INLINE float idVecX::NormalizeSelf() {
+	float sum = 0.0f;
+	for ( int i = 0; i < size; i++ ) {
+		sum += p[i] * p[i];
+	}
+	float invSqrt = idMath::InvSqrt( sum );
+	for ( int i = 0; i < size; i++ ) {
+		p[i] *= invSqrt;
+	}
+	return invSqrt * sum;
+}
+
+/*
+========================
+idVecX::GetDimension
+========================
+*/
+ID_INLINE int idVecX::GetDimension() const {
+	return size;
+}
+
+/*
+========================
+idVecX::SubVec3
+========================
+*/
+ID_INLINE idVec3 &idVecX::SubVec3( int index ) {
+	assert( index >= 0 && index * 3 + 3 <= size );
+	return *reinterpret_cast<idVec3 *>(p + index * 3);
+}
+
+/*
+========================
+idVecX::SubVec3
+========================
+*/
+ID_INLINE const idVec3 &idVecX::SubVec3( int index ) const {
+	assert( index >= 0 && index * 3 + 3 <= size );
+	return *reinterpret_cast<const idVec3 *>(p + index * 3);
+}
+
+/*
+========================
+idVecX::SubVec6
+========================
+*/
+ID_INLINE idVec6 &idVecX::SubVec6( int index ) {
+	assert( index >= 0 && index * 6 + 6 <= size );
+	return *reinterpret_cast<idVec6 *>(p + index * 6);
+}
+
+/*
+========================
+idVecX::SubVec6
+========================
+*/
+ID_INLINE const idVec6 &idVecX::SubVec6( int index ) const {
+	assert( index >= 0 && index * 6 + 6 <= size );
+	return *reinterpret_cast<const idVec6 *>(p + index * 6);
+}
+
+/*
+========================
+idVecX::ToFloatPtr
+========================
+*/
+ID_INLINE const float *idVecX::ToFloatPtr() const {
+	return p;
+}
+
+/*
+========================
+idVecX::ToFloatPtr
+========================
+*/
+ID_INLINE float *idVecX::ToFloatPtr() {
+	return p;
+}
+
+/*
+========================
+idVecX::AddScaleAdd
+========================
+*/
+ID_INLINE void idVecX::AddScaleAdd( const float scale, const idVecX &v0, const idVecX &v1 ) {
+	assert( GetSize() == v0.GetSize() );
+	assert( GetSize() == v1.GetSize() );
+
+	const float * v0Ptr = v0.ToFloatPtr();
+	const float * v1Ptr = v1.ToFloatPtr();
+	float * dstPtr = ToFloatPtr();
+
+	for ( int i = 0; i < size; i++ ) {
+		dstPtr[i] += scale * ( v0Ptr[i] + v1Ptr[i] );
+	}
+}
+
+#endif // !__MATH_VECTORX_H__

neo/idlib/math/Vector.cpp

@@ -0,0 +1,377 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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.
+
+===========================================================================
+*/
+
+#pragma hdrstop
+#include "../precompiled.h"
+
+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() 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() 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() 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() 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() 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 );
+}

neo/idlib/math/VectorI.h

@@ -0,0 +1,97 @@
+/*
+===========================================================================
+
+Doom 3 BFG Edition GPL Source Code
+Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
+
+This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
+
+Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
+
+In addition, the Doom 3 BFG Edition 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 BFG Edition 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_VECTORI_H__
+#define __MATH_VECTORI_H__
+
+static ID_INLINE int MinInt( int a, int b ) { return (a) < (b) ? (a) : (b); }
+static ID_INLINE int MaxInt( int a, int b ) { return (a) < (b) ? (b) : (a); }
+
+class idVec2i {
+public:
+    int      x, y;
+
+	idVec2i() {}
+	idVec2i( int _x, int _y ) : x(_x), y(_y ) {}
+
+	void		Set( int _x, int _y ) { x = _x; y = _y; }
+	int			Area() const { return x * y; };
+
+	void		Min( idVec2i &v ) { x = MinInt( x, v.x ); y = MinInt( y, v.y ); }
+	void		Max( idVec2i &v ) { x = MaxInt( x, v.x ); y = MaxInt( y, v.y ); }
+
+	int			operator[]( const int index ) const { assert( index == 0 || index == 1 ); return (&x)[index]; }
+	int &		operator[]( const int index ) { assert( index == 0 || index == 1 ); return (&x)[index]; }
+
+	idVec2i 	operator-() const { return idVec2i( -x, -y ); }
+	idVec2i 	operator!() const { return idVec2i( !x, !y ); }
+
+	idVec2i 	operator>>( const int a ) const { return idVec2i( x >> a, y >> a ); }
+	idVec2i 	operator<<( const int a ) const { return idVec2i( x << a, y << a ); }
+	idVec2i 	operator&( const int a ) const { return idVec2i( x & a, y & a ); }
+	idVec2i 	operator|( const int a ) const { return idVec2i( x | a, y | a ); }
+	idVec2i 	operator^( const int a ) const { return idVec2i( x ^ a, y ^ a ); }
+	idVec2i 	operator*( const int a ) const { return idVec2i( x * a, y * a ); }
+	idVec2i 	operator/( const int a ) const { return idVec2i( x / a, y / a ); }
+	idVec2i 	operator+( const int a ) const { return idVec2i( x + a, y + a ); }
+	idVec2i 	operator-( const int a ) const { return idVec2i( x - a, y - a ); }
+
+	bool		operator==( const idVec2i &a ) const { return a.x == x && a.y == y; };
+	bool		operator!=( const idVec2i &a ) const { return a.x != x || a.y != y; };
+
+	idVec2i		operator>>( const idVec2i &a ) const { return idVec2i( x >> a.x, y >> a.y ); }
+	idVec2i		operator<<( const idVec2i &a ) const { return idVec2i( x << a.x, y << a.y ); }
+	idVec2i		operator&( const idVec2i &a ) const { return idVec2i( x & a.x, y & a.y ); }
+	idVec2i		operator|( const idVec2i &a ) const { return idVec2i( x | a.x, y | a.y ); }
+	idVec2i		operator^( const idVec2i &a ) const { return idVec2i( x ^ a.x, y ^ a.y ); }
+	idVec2i		operator*( const idVec2i &a ) const { return idVec2i( x * a.x, y * a.y ); }
+	idVec2i		operator/( const idVec2i &a ) const { return idVec2i( x / a.x, y / a.y ); }
+	idVec2i		operator+( const idVec2i &a ) const { return idVec2i( x + a.x, y + a.y ); }
+	idVec2i		operator-( const idVec2i &a ) const { return idVec2i( x - a.x, y - a.y ); }
+
+	idVec2i &	operator+=( const int a ) { x += a; y += a; return *this; }
+	idVec2i &	operator-=( const int a ) { x -= a; y -= a; return *this; }
+	idVec2i &	operator/=( const int a ) { x /= a; y /= a; return *this; }
+	idVec2i &	operator*=( const int a ) { x *= a; y *= a; return *this; }
+	idVec2i &	operator>>=( const int a ) { x >>= a; y >>= a; return *this; }
+	idVec2i &	operator<<=( const int a ) { x <<= a; y <<= a; return *this; }
+	idVec2i &	operator&=( const int a ) { x &= a; y &= a; return *this; }
+	idVec2i &	operator|=( const int a ) { x |= a; y |= a; return *this; }
+	idVec2i &	operator^=( const int a ) { x ^= a; y ^= a; return *this; }
+
+	idVec2i &	operator>>=( const idVec2i &a ) { x >>= a.x; y >>= a.y; return *this; }
+	idVec2i &	operator<<=( const idVec2i &a ) { x <<= a.x; y <<= a.y; return *this; }
+	idVec2i &	operator&=( const idVec2i &a ) { x &= a.x; y &= a.y; return *this; }
+	idVec2i &	operator|=( const idVec2i &a ) { x |= a.x; y |= a.y; return *this; }
+	idVec2i &	operator^=( const idVec2i &a ) { x ^= a.x; y ^= a.y; return *this; }
+	idVec2i &	operator+=( const idVec2i &a ) { x += a.x; y += a.y; return *this; }
+	idVec2i &	operator-=( const idVec2i &a ) { x -= a.x; y -= a.y; return *this; }
+	idVec2i &	operator/=( const idVec2i &a ) { x /= a.x; y /= a.y; return *this; }
+	idVec2i &	operator*=( const idVec2i &a ) { x *= a.x; y *= a.y; return *this; }
+};
+
+#endif