source-engine/vphysics/convert.cpp

366 lines
9.7 KiB
C++
Raw Normal View History

2020-04-22 16:56:21 +00:00
//========= 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;
}