1HEADmaster

1 files changed, 365 insertions, 0 deletions

diff --git a/vphysics/convert.cpp b/vphysics/convert.cpp
new file mode 100644
index 0000000..5204104
--- /dev/null
+++ b/vphysics/convert.cpp
@@ -0,0 +1,365 @@
+//========= Copyright Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//=============================================================================//
+#include <stdio.h>
+#include "convert.h"
+#include "ivp_cache_object.hxx"
+#include "coordsize.h"
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+#if 1
+// game is in inches
+vphysics_units_t g_PhysicsUnits = 
+{ 
+	METERS_PER_INCH, //float		unitScaleMeters;			// factor that converts game units to meters
+	1.0f / METERS_PER_INCH, //float		unitScaleMetersInv;		// factor that converts meters to game units
+	0.25f,			// float		globalCollisionTolerance;	// global collision tolerance in game units
+	DIST_EPSILON,	// float		collisionSweepEpsilon;		// collision sweep tests clip at this, must be the same as engine's DIST_EPSILON
+	1.0f/256.0f,	// float		collisionSweepIncrementalEpsilon;	// near-zero test for incremental steps in collision sweep tests
+};
+#else
+// game is in meters
+vphysics_units_t g_PhysicsUnits = 
+{ 
+	1.0f,			//float		unitScaleMeters;			// factor that converts game units to meters
+	1.0f,			//float		unitScaleMetersInv;			// factor that converts meters to game units
+	0.01f,			// float		globalCollisionTolerance;	// global collision tolerance in game units
+	0.01f,			// float		collisionSweepEpsilon;		// collision sweep tests clip at this, must be the same as engine's DIST_EPSILON
+	1e-4f,			// float		collisionSweepIncrementalEpsilon;	// near-zero test for incremental steps in collision sweep tests
+};
+#endif
+
+//-----------------------------------------------------------------------------
+// HL to IVP conversions
+//-----------------------------------------------------------------------------
+
+void ConvertBoxToIVP( const Vector &mins, const Vector &maxs, Vector &outmins, Vector &outmaxs )
+{
+	float tmpZ;
+
+	tmpZ = mins.y;
+	outmins.y = -HL2IVP(mins.z);
+	outmins.z = HL2IVP(tmpZ);
+	outmins.x = HL2IVP(mins.x);
+	tmpZ = maxs.y;
+	outmaxs.y = -HL2IVP(maxs.z);
+	outmaxs.z = HL2IVP(tmpZ);
+	outmaxs.x = HL2IVP(maxs.x);
+
+	tmpZ = outmaxs.y;
+	outmaxs.y = outmins.y;
+	outmins.y = tmpZ;
+}
+
+
+void ConvertMatrixToIVP( const matrix3x4_t& matrix, IVP_U_Matrix &out )
+{
+	Vector forward, left, up;
+
+	forward.x = matrix[0][0];
+	forward.y = matrix[1][0];
+	forward.z = matrix[2][0];
+
+	left.x = matrix[0][1];
+	left.y = matrix[1][1];
+	left.z = matrix[2][1];
+
+	up.x = matrix[0][2];
+	up.y = matrix[1][2];
+	up.z = matrix[2][2];
+
+	up = -up;
+
+	IVP_U_Float_Point ivpForward, ivpLeft, ivpUp;
+
+	ConvertDirectionToIVP( forward, ivpForward );
+	ConvertDirectionToIVP( left, ivpLeft );
+	ConvertDirectionToIVP( up, ivpUp );
+
+	out.set_col( IVP_INDEX_X, &ivpForward );
+	out.set_col( IVP_INDEX_Z, &ivpLeft );
+	out.set_col( IVP_INDEX_Y, &ivpUp );
+
+	out.vv.k[0] = HL2IVP(matrix[0][3]);
+	out.vv.k[1] = -HL2IVP(matrix[2][3]);
+	out.vv.k[2] = HL2IVP(matrix[1][3]);
+}
+
+
+void ConvertRotationToIVP( const QAngle& angles, IVP_U_Matrix3 &out )
+{
+	Vector forward, right, up;
+	IVP_U_Float_Point ivpForward, ivpLeft, ivpUp;
+	
+	AngleVectors( angles, &forward, &right, &up );
+	// now this is left
+	right = -right;
+
+	up = -up;
+	
+	ConvertDirectionToIVP( forward, ivpForward );
+	ConvertDirectionToIVP( right, ivpLeft );
+	ConvertDirectionToIVP( up, ivpUp );
+	
+	out.set_col( IVP_INDEX_X, &ivpForward );
+	out.set_col( IVP_INDEX_Z, &ivpLeft );
+	out.set_col( IVP_INDEX_Y, &ivpUp );
+}
+
+void ConvertRotationToIVP( const QAngle& angles, IVP_U_Quat &out )
+{
+	IVP_U_Matrix3 tmp;
+	ConvertRotationToIVP( angles, tmp );
+	out.set_quaternion( &tmp );
+}
+
+//-----------------------------------------------------------------------------
+// IVP to HL conversions
+//-----------------------------------------------------------------------------
+
+void ConvertMatrixToHL( const IVP_U_Matrix &in, matrix3x4_t& output )
+{
+#if 1
+	// copy the row vectors over, swapping z & -y.  Also, negate output z
+	output[0][0] = in.get_elem(0, 0);
+	output[0][2] = -in.get_elem(0, 1);
+	output[0][1] = in.get_elem(0, 2);
+
+	output[1][0] = in.get_elem(2, 0);
+	output[1][2] = -in.get_elem(2, 1);
+	output[1][1] = in.get_elem(2, 2);
+
+	output[2][0] = -in.get_elem(1, 0);
+	output[2][2] = in.get_elem(1, 1);
+	output[2][1] = -in.get_elem(1, 2);
+
+#else
+
+	// this code is conceptually simpler, but the above is smaller/faster
+	Vector forward, left, up;
+	IVP_U_Float_Point out;
+
+	in.get_col( IVP_INDEX_X, &out );
+	ConvertDirectionToHL( out, forward );
+	in.get_col( IVP_INDEX_Z, &out );
+	ConvertDirectionToHL( out, left);
+	in.get_col( IVP_INDEX_Y, &out );
+	ConvertDirectionToHL( out, up );
+	up = -up;
+
+	output[0][0] = forward.x;
+	output[1][0] = forward.y;
+	output[2][0] = forward.z;
+
+	output[0][1] = left.x;
+	output[1][1] = left.y;
+	output[2][1] = left.z;
+
+	output[0][2] = up.x;
+	output[1][2] = up.y;
+	output[2][2] = up.z;
+#endif
+	output[0][3] = IVP2HL(in.vv.k[0]);
+	output[1][3] = IVP2HL(in.vv.k[2]);
+	output[2][3] = -IVP2HL(in.vv.k[1]);
+}
+
+
+void ConvertRotationToHL( const IVP_U_Matrix3 &in, QAngle& angles )
+{
+	IVP_U_Float_Point out;
+	Vector forward, right, up;
+
+	in.get_col( IVP_INDEX_X, &out );
+	ConvertDirectionToHL( out, forward );
+	in.get_col( IVP_INDEX_Z, &out );
+	ConvertDirectionToHL( out, right );
+	in.get_col( IVP_INDEX_Y, &out );
+	ConvertDirectionToHL( out, up );
+
+	float xyDist = sqrt( forward[0] * forward[0] + forward[1] * forward[1] );
+	
+	// enough here to get angles?
+	if ( xyDist > 0.001 )
+	{
+		// (yaw)	y = ATAN( forward.y, forward.x );		-- in our space, forward is the X axis
+		angles[1] = RAD2DEG( atan2( forward[1], forward[0] ) );
+
+		// (pitch)	x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
+		angles[0] = RAD2DEG( atan2( -forward[2], xyDist ) );
+
+		// (roll)	z = ATAN( -right.z, up.z );
+		angles[2] = RAD2DEG( atan2( -right[2], up[2] ) ) + 180;
+	}
+	else	// forward is mostly Z, gimbal lock
+	{
+		// (yaw)	y = ATAN( -right.x, right.y );		-- forward is mostly z, so use right for yaw
+		angles[1] = RAD2DEG( atan2( right[0], -right[1] ) );
+
+		// (pitch)	x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
+		angles[0] = RAD2DEG( atan2( -forward[2], xyDist ) );
+
+		// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll)
+		angles[2] = 180;
+	}
+}
+
+
+void ConvertRotationToHL( const IVP_U_Quat &in, QAngle& angles )
+{
+	IVP_U_Matrix3 tmp;
+	in.set_matrix( &tmp );
+	ConvertRotationToHL( tmp, angles );
+}
+
+// utiltiy code
+void TransformIVPToLocal( IVP_U_Point &point, IVP_Real_Object *pObject, bool translate )
+{
+	IVP_U_Point tmp = point;
+	TransformIVPToLocal( tmp, point, pObject, translate );
+}
+
+void TransformLocalToIVP( IVP_U_Point &point, IVP_Real_Object *pObject, bool translate )
+{
+	IVP_U_Point tmp = point;
+	TransformLocalToIVP( tmp, point, pObject, translate );
+}
+
+
+// UNDONE: use IVP_Cache_Object instead?  Measure perf differences.
+#define USE_CACHE_OBJECT	0
+
+
+//-----------------------------------------------------------------------------
+// Purpose: This is ONLY for use by the routines below.  It's not reentrant!!!
+//			No threads or recursive calls!
+//-----------------------------------------------------------------------------
+#if USE_CACHE_OBJECT
+#else
+static const IVP_U_Matrix *GetTmpObjectMatrix( IVP_Real_Object *pObject )
+{
+	static IVP_U_Matrix coreShiftMatrix;
+	const IVP_U_Matrix *pOut = pObject->get_core()->get_m_world_f_core_PSI();
+
+	if ( !pObject->flags.shift_core_f_object_is_zero )
+	{
+		coreShiftMatrix.set_matrix( pOut );
+		coreShiftMatrix.vmult4( pObject->get_shift_core_f_object(), &coreShiftMatrix.vv );
+		return &coreShiftMatrix;
+	}
+	return pOut;
+}
+#endif
+
+void TransformIVPToLocal( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate )
+{
+#if USE_CACHE_OBJECT
+	IVP_Cache_Object *cache = pObject->get_cache_object_no_lock();
+
+	if ( translate )
+	{
+		cache->transform_position_to_object_coords( &pointIn, &pointOut );
+	}
+	else
+	{
+		cache->transform_vector_to_object_coords( &pointIn, &pointOut );
+	}
+#else
+	const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject );
+	if ( translate )
+	{
+		pMatrix->inline_vimult4( &pointIn, &pointOut );
+	}
+	else
+	{
+		pMatrix->inline_vimult3( &pointIn, &pointOut );
+	}
+#endif
+}
+
+
+void TransformLocalToIVP( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate )
+{
+#if USE_CACHE_OBJECT
+	IVP_Cache_Object *cache = pObject->get_cache_object_no_lock();
+
+	if ( translate )
+	{
+		IVP_U_Float_Point floatPointIn;
+		floatPointIn.set( &pointIn );
+		cache->transform_position_to_world_coords( &floatPointIn, &pointOut );
+	}
+	else
+	{
+		cache->transform_vector_to_world_coords( &pointIn, &pointOut );
+	}
+#else
+	const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject );
+
+	if ( translate )
+	{
+		pMatrix->inline_vmult4( &pointIn, &pointOut );
+	}
+	else
+	{
+		pMatrix->inline_vmult3( &pointIn, &pointOut );
+	}
+#endif
+}
+
+void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate )
+{
+#if USE_CACHE_OBJECT
+	IVP_Cache_Object *cache = pObject->get_cache_object_no_lock();
+
+	if ( translate )
+	{
+		cache->transform_position_to_world_coords( &pointIn, &pointOut );
+	}
+	else
+	{
+		IVP_U_Point doublePointIn;
+		doublePointIn.set( &pointIn );
+		cache->transform_vector_to_world_coords( &doublePointIn, &pointOut );
+	}
+#else
+	const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject );
+	IVP_U_Float_Point out;
+
+	if ( translate )
+	{
+		pMatrix->inline_vmult4( &pointIn, &out );
+	}
+	else
+	{
+		pMatrix->inline_vmult3( &pointIn, &out );
+	}
+	pointOut.set( &out );
+#endif
+}
+
+void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Float_Point &pointOut, IVP_Real_Object *pObject, bool translate )
+{
+	IVP_U_Point tmpOut;
+	TransformLocalToIVP( pointIn, tmpOut, pObject, translate );
+	pointOut.set( &tmpOut );
+}
+
+static char axisMap[] = {0,2,1,3};
+
+int ConvertCoordinateAxisToIVP( int axisIndex )
+{
+	return axisIndex < 4 ? axisMap[axisIndex] : 0;
+}
+
+int ConvertCoordinateAxisToHL( int axisIndex )
+{
+	return axisIndex < 4 ? axisMap[axisIndex] : 0;
+}
+